navigation.py 2.76 KB
Newer Older
JoelMXLinux's avatar
JoelMXLinux committed
1
2
3
4
5
6
""" 
The main class for the yoshi vector. The elementary logic is implemented here, 
while most utility functions are located in support.py
"""


JoelMXLinux's avatar
JoelMXLinux committed
7
import anki_vector
Felix Schreyer's avatar
Felix Schreyer committed
8
9
from support import *
import support
JoelMXLinux's avatar
JoelMXLinux committed
10
11
from anki_vector.util import degrees, distance_mm, speed_mmps
from anki_vector.connection import ControlPriorityLevel
JoelMXLinux's avatar
JoelMXLinux committed
12
from anki_vector import behavior
JoelMXLinux's avatar
JoelMXLinux committed
13

Felix Schreyer's avatar
Felix Schreyer committed
14
BALLOON_SIZE_MM = 100
Felix Schreyer's avatar
Felix Schreyer committed
15
PICTURE_PATH = './balloon_pic.jpg'
JoelMXLinux's avatar
JoelMXLinux committed
16
MAX_DRIVING_DISTANCE = 1500;
Felix Schreyer's avatar
Felix Schreyer committed
17
AZURE_CONFIG_FILE = './azure_config.txt'
JoelMXLinux's avatar
JoelMXLinux committed
18
19
IP_ADDRESS = '192.168.0.106'
SERIAL = '008014c1'
nico.bec's avatar
nico.bec committed
20
ROBOT_HEIGHT = 657
JoelMXLinux's avatar
JoelMXLinux committed
21
22
probability = 0.5

Felix Schreyer's avatar
bugfix    
Felix Schreyer committed
23

Felix Schreyer's avatar
Felix Schreyer committed
24
def search(robot, predictor):
JoelMXLinux's avatar
JoelMXLinux committed
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
    """ 
    Keeps turning around and looking for the ballon/robot. 
    If there has been no result for 4 rotations (more than 360°) 
    the robot moves a few centimeters to be a harder target
    
    Parameters
    ----------
    robot: anki_vector.Robot
        The robot instance that should be controlled
    
    predictor: support.img_prediction or support.offline_img_preiction
        An initialized instance that can make predictions, using the azure cloud or alternatively GCV.
    
    
    Returns
    -------
    json
        The bounding boxes of detected robots/balloons
    
    """

Felix Schreyer's avatar
Felix Schreyer committed
46
    result_of_search = None
Joel's avatar
Joel committed
47
    
JoelMXLinux's avatar
JoelMXLinux committed
48
    i = 0
Joel's avatar
Joel committed
49

Felix Schreyer's avatar
Felix Schreyer committed
50
    while result_of_search is None:
JoelMXLinux's avatar
JoelMXLinux committed
51
52
53
54
        if i>2:
            robot.behavior.drive_straight(distance_mm(150), speed_mmps(250))
            i=0
        i += 1
55
        print('Keep Searching, taking new picture')
nico.bec's avatar
nico.bec committed
56
        
nico.bec's avatar
nico.bec committed
57
        robot.behavior.turn_in_place(degrees(96))
58
59
        result_of_search = evaluate_picture(robot, predictor, BALLOON_SIZE_MM)
        print('Result of Search: ', result_of_search)
Felix Schreyer's avatar
Felix Schreyer committed
60
    return result_of_search
Felix Schreyer's avatar
bugfix    
Felix Schreyer committed
61

JoelMXLinux's avatar
JoelMXLinux committed
62
if __name__ == '__main__':
JoelMXLinux's avatar
JoelMXLinux committed
63
64
65
66
67
68
69
70
71
72
    """ 
    Main functions that controls the fundamental behaviour.
    First the robot is initialized with maximum priority and surpressed idle behaviour. 
    In a loop:
        Take initial picture
        If no balloon found:
            Keeps searching for an robot/balloon. 
        When a balloon is found, the position is estimated. 
        Then calling a method to drive to the calculated position.
    """
JoelMXLinux's avatar
JoelMXLinux committed
73
    args = anki_vector.util.parse_command_args()
JoelMXLinux's avatar
JoelMXLinux committed
74
    with behavior.ReserveBehaviorControl(serial = SERIAL, ip=IP_ADDRESS):
JoelMXLinux's avatar
JoelMXLinux committed
75

JoelMXLinux's avatar
JoelMXLinux committed
76
        with anki_vector.Robot(serial = SERIAL, name='Vector-N8G2', ip=IP_ADDRESS,
JoelMXLinux's avatar
JoelMXLinux committed
77
                            behavior_control_level=ControlPriorityLevel.OVERRIDE_BEHAVIORS_PRIORITY) as robot:
Felix Schreyer's avatar
Felix Schreyer committed
78
79
80
            robot_initiate(robot)

            predictor = img_prediction(AZURE_CONFIG_FILE)
JoelMXLinux's avatar
JoelMXLinux committed
81
82


JoelMXLinux's avatar
JoelMXLinux committed
83
            while True:
84
                result = evaluate_picture(robot, predictor, BALLOON_SIZE_MM)
Felix Schreyer's avatar
Felix Schreyer committed
85
                if result is None:
Felix Schreyer's avatar
Felix Schreyer committed
86
                    result = search(robot, predictor)
Felix Schreyer's avatar
Felix Schreyer committed
87

JoelMXLinux's avatar
JoelMXLinux committed
88
                support.drive_towards_pose(robot, result, MAX_DRIVING_DISTANCE)
nico.bec's avatar
nico.bec committed
89