eduro/sickday2016.py at master · luciusb/eduro · 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
#!/usr/bin/python
"""
  SICK Robot Day 2016 main program
    usage:
       ./sickday.py <note or cmd:<test command>> [--file <log file> [F|FF]]
"""

import sys
import math
import os
import random
from itertools import izip, islice

try: # Use Psyco, if available.
    import psyco
    psyco.full()
except ImportError:
    pass

from eduro import EmergencyStopException
from eduromaxi import EduroMaxi
from driver import Driver, normalizeAnglePIPI, angleTo
from localisation import SimpleOdometry

from can import CAN, ReplyLog, ReplyLogInputsOnly

import viewlog
from viewlog import viewLogExtension, viewCompassExtension, viewPoseExtension

from ray_trace import combinedPose
from line import distance, Line
from route import loadLatLonPts, Convertor


import starter


def setupGripperModule(can):
    pass  # TODO setup the servo PWM output


class SICKRobotDay2016:
    def __init__(self, robot, code, verbose = False):
        self.random = random.Random(0).uniform
        self.robot = robot
        self.verbose = verbose
        self.code = code
        self.robot.attachEmergencyStopButton()

        # do we want some processing?
        self.robot.attachCamera( cameraExe = "../digits/digits",
                url = "http://192.168.0.99/image?res=full&x0=352&y0=80&x1=992&y1=592&quality=12&doublescan=0" )
#        self.robot.addExtension( cameraDataExtension )
        self.robot.attachLaser( pose=((0.14, 0.0, 0.32), (0,0,0)) )
#        self.robot.attachLaser( index=2, remission=True, usb=True,
#                pose=((0.19, 0.0, 0.055), tuple([math.radians(x) for x in (0, 180, 0)])),
#                errLog = self.robot.metaLog )

        self.driver = Driver( self.robot, maxSpeed = 0.5, maxAngularSpeed = math.radians(180) )
        self.robot.localisation = SimpleOdometry()

        self.robot.laser.stopOnExit = False    # for faster boot-up


    def run( self ):
        try:
            if getattr( self.robot.laser, 'startLaser', None ):
                # trigger rotation of the laser, low level function, ignore for log files
                print "Powering laser ON"
                self.robot.laser.startLaser()
            self.robot.waitForStart()
            self.robot.laser.start()    # laser also after start -- it should be already running
#            self.robot.laser2.start()
            self.robot.camera.start()
            self.robot.localisation = SimpleOdometry()
            while True:
                self.ver0(verbose = self.verbose)
        except EmergencyStopException, e:
            print "EmergencyStopException"
        self.robot.laser.requestStop()
#        self.robot.laser2.requestStop()
        self.robot.camera.requestStop()


    def ver0( self, verbose=False ):
        # Go straight for 2 meters
        print "ver0", self.robot.battery

        self.driver.goStraight(2.0, timeout=30)

        print 'Game over.', self.robot.battery
        for k in xrange(10):
            self.robot.setSpeedPxPa(0.0, 0.0)
            self.robot.update()
        raise EmergencyStopException() # TODO: Introduce GameOverException as in Eurobot


    def __call__( self ):
        print "RUNNING:", self.code
        if self.code.startswith("cmd:"):
            return eval(self.code[4:])
        return self.run()


if __name__ == "__main__":
    from eduromaxi import EduroMaxi
    import launcher
    launcher.launch(sys.argv, EduroMaxi, SICKRobotDay2016, configFn=setupGripperModule)

# vim: expandtab sw=4 ts=4