forked from robotika/eduro
-
Notifications
You must be signed in to change notification settings - Fork 0
/
sickday2016.py
111 lines (84 loc) · 3.43 KB
/
sickday2016.py
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