def navigate_pattern(metalog, conf, viewer=None):
assert metalog is not None
can_log_name = metalog.getLog('can')
if metalog.replay:
if metalog.areAssertsEnabled():
can = CAN(ReplayLog(can_log_name), skipInit=True)
else:
can = CAN(ReplayLogInputsOnly(can_log_name), skipInit=True)
else:
can = CAN()
can.relog(can_log_name,
timestamps_log=open(metalog.getLog('timestamps'), 'w'))
can.resetModules(configFn=setup_faster_update)
if conf is not None and 'localization' in conf.data:
loc = SimpleOdometry.from_dict(conf.data['localization'])
else:
loc = SimpleOdometry(pose=(0.0, 2.5, 0.0))
loc.global_map = [(0.0, 0.0), (15.0, 0.0), (15.0, 5.0), (0.0, 5.0)]
jd_config = None
if conf is not None and 'johndeere' in conf.data:
jd_config = conf.data['johndeere']
robot = JohnDeere(can=can, localization=loc, config=jd_config)
robot.UPDATE_TIME_FREQUENCY = 20.0 # TODO change internal and integrate setup
for sensor_name in ['gps', 'laser', 'camera']:
attach_sensor(robot, sensor_name, metalog)
attach_processor(robot, metalog, image_callback)
long_side = max([x for x, y in robot.localization.global_map])
robot.canproxy.stop()
robot.canproxy.set_turn_raw(0)
if viewer is not None:
robot.extensions.append(('viewer', viewer_extension))
speed = 0.5
try:
robot.extensions.append(('detect_near', detect_near_extension))
robot.extensions.append(('emergency_stop', emergency_stop_extension))
for i in xrange(10):
# run_oval(robot, speed)
run_there_and_back(robot, long_side, speed)
except NearObstacle:
print "Near Exception Raised!"
robot.extensions = [] # hack
except EmergencyStopException:
print "Emergency STOP Exception!"
robot.extensions = [] # hack
robot.canproxy.stop()
robot.canproxy.stop_turn()
robot.wait(3.0)
detach_all_sensors(robot)
def self_test(metalog):
assert metalog is not None
can_log_name = metalog.getLog('can')
if metalog.replay:
if metalog.areAssertsEnabled():
can = CAN(ReplayLog(can_log_name), skipInit=True)
else:
can = CAN(ReplayLogInputsOnly(can_log_name), skipInit=True)
else:
can = CAN()
can.relog(can_log_name)
can.resetModules(configFn=setup_faster_update)
robot = JohnDeere(can=can)
robot.UPDATE_TIME_FREQUENCY = 20.0 # TODO change internal and integrate setup
robot.canproxy.stop() # center(robot)
wait_for_start(robot)
robot.desired_speed = 0.5
start_time = robot.time
robot.canproxy.set_turn_raw(0)
robot.canproxy.go() # go(robot)
while robot.time - start_time < 333.0:
robot.update()
print robot.time, robot.gas
if not robot.buttonGo:
print "STOP!"
break
robot.canproxy.stop_turn()
center(robot) # keep old version for the first test
def driver_self_test(driver, metalog):
assert metalog is not None
can_log_name = metalog.getLog('can')
if metalog.replay:
if metalog.areAssertsEnabled():
can = CAN(ReplayLog(can_log_name), skipInit=True)
else:
can = CAN(ReplayLogInputsOnly(can_log_name), skipInit=True)
else:
can = CAN()
can.relog(can_log_name)
can.resetModules(configFn=setup_faster_update)
robot = JohnDeere(can=can)
robot.UPDATE_TIME_FREQUENCY = 20.0 # TODO change internal and integrate setup
robot.localization = None # TODO
robot.canproxy.stop()
robot.canproxy.set_turn_raw(0)
go_straight(robot, distance=1.0, speed=0.3, with_stop=True)
for i in xrange(3):
turn(robot, math.radians(-45), radius=2.6, speed=0.5)
turn(robot, math.radians(-45), radius=2.6, speed=-0.5)
robot.canproxy.stop_turn()
robot.wait(3.0)
def testNewSDO_0( filename ):
from can import ReplyLog
can=CAN(ReplyLog(filename))
can.resetModules()
sdoplg = SDOPlg( can )
( nodeID, dataIndex, subIndex ) = 10, 16*256+10, 0
data = sdoplg.readSDO( nodeID, dataIndex, subIndex )
print("RESUT DATA:", data)
def testNewSDO_0(filename):
from can import ReplyLog
can = CAN(ReplyLog(filename))
can.resetModules()
sdoplg = SDOPlg(can)
(nodeID, dataIndex, subIndex) = 10, 16 * 256 + 10, 0
data = sdoplg.readSDO(nodeID, dataIndex, subIndex)
print("RESUT DATA:", data)
def main( argv ):
from can import ReplyLog, DummyMemoryLog, ReplyLogInputsOnly
if len( argv ) < 2:
r=Robot()
elif len( argv ) == 2:
r=Robot( CAN(ReplyLog( argv[1] ), skipInit = True ) )
else:
can = CAN( com = DummyMemoryLog() ) # skip CAN initialization
can.com = ReplyLogInputsOnly( argv[1] ) # switch to log file
r = Robot( can )
# r.localisation = localisation.SimpleOdometry()
r.localisation = localisation.KalmanFilter()
print r.localisation
# try:
# while 1:
for i in range(10000):
r.update()
print r.compass
# print r.prevEncL
#except : #ignore termination
# pass
print r.localisation.pose()
return
s = "AHOJ jak se mas? Tototo je prvni dlouhy text. ".upper() * 2
r.beep = 1
for i in range( len(s)-2 ):
r.toDisplay = s[i:i+2]
for j in range(7):
r.update()
r.beep = 0
r.update()
del r
return
# wait for start cable to be removed
while r.startCableIn is None or r.startCableIn == True:
r.update()
r.setSpeedPxPa( 0.1, angleDeg(0) )
r.update()
r.update()
r.update()
assert( r.compass != None )
for i in range(60):
r.update()
# print "Compass", r.compass
print "Sharps", r.sharp
del r
def main(argv):
from can import ReplyLog, DummyMemoryLog, ReplyLogInputsOnly
if len(argv) < 2:
r = Robot()
elif len(argv) == 2:
r = Robot(CAN(ReplyLog(argv[1]), skipInit=True))
else:
can = CAN(com=DummyMemoryLog()) # skip CAN initialization
can.com = ReplyLogInputsOnly(argv[1]) # switch to log file
r = Robot(can)
# r.localisation = localisation.SimpleOdometry()
r.localisation = localisation.KalmanFilter()
print r.localisation
# try:
# while 1:
for i in range(10000):
r.update()
print r.compass
# print r.prevEncL
# except : #ignore termination
# pass
print r.localisation.pose()
return
s = "AHOJ jak se mas? Tototo je prvni dlouhy text. ".upper() * 2
r.beep = 1
for i in range(len(s) - 2):
r.toDisplay = s[i : i + 2]
for j in range(7):
r.update()
r.beep = 0
r.update()
del r
return
# wait for start cable to be removed
while r.startCableIn is None or r.startCableIn == True:
r.update()
r.setSpeedPxPa(0.1, angleDeg(0))
r.update()
r.update()
r.update()
assert r.compass != None
for i in range(60):
r.update()
# print "Compass", r.compass
print "Sharps", r.sharp
del r
def __init__( self, can=None, maxAcc = 1.0, replyLog = None, metaLog = None ):
self.metaLog = metaLog
self.replyLog = replyLog
if not can:
can=CAN()
can.resetModules()
print can
self.can = can
self.WHEEL_DISTANCE = 0.245 # small Eduro
self._WHEEL_DIAMETER = 0.15
self.maxAcc = maxAcc # could be None as "no limit"
self.maxAngularAcc = 2.0*maxAcc/self.WHEEL_DISTANCE
self.swappedMotors = False
self.SpeedL,self.SpeedR=0,0
self.prevEncL = None
self.prevEncR = None
self.encData = {}
self._rampLastLeft, self._rampLastRight = 0.0, 0.0
self.lastDistStep = 0.0
self.lastAngleStep = 0.0
self.currentSpeed = 0.0
self.currentAngularSpeed = 0.0
self.time = 0.0 # time in seconds since boot
self.battery = None
self.emergencyStop = None
self.startCableIn = None
self.switchBlueSelected = None
self.buttonPause = None
self.cmdLEDBlue = 0xFF
self.cmdLEDRed = 0xFF
self.toDisplay = None
self.beep = None
self.compass = None
self.compassRaw = None
self.compassAcc = None
self.compassAccRaw = None
self.sharp = None
self.fnSpeedLimitController = [self.sharpSpeedFn, self.pauseSpeedFn]
self.localisation = None
self.extensions=[]
self.dataSources=[]
self.modulesForRestart = []
self.can.sendOperationMode()
def __init__(self, can=None, maxAcc=1.0, replyLog=None, metaLog=None):
self.metaLog = metaLog
self.replyLog = replyLog
if not can:
can = CAN()
can.resetModules()
print can
self.can = can
self.WHEEL_DISTANCE = 0.245 # small Eduro
self._WHEEL_DIAMETER = 0.15
self.maxAcc = maxAcc # could be None as "no limit"
self.maxAngularAcc = 2.0 * maxAcc / self.WHEEL_DISTANCE
self.swappedMotors = False
self.SpeedL, self.SpeedR = 0, 0
self.prevEncL = None
self.prevEncR = None
self.encData = {}
self._rampLastLeft, self._rampLastRight = 0.0, 0.0
self.lastDistStep = 0.0
self.lastAngleStep = 0.0
self.currentSpeed = 0.0
self.currentAngularSpeed = 0.0
self.time = 0.0 # time in seconds since boot
self.battery = None
self.emergencyStop = None
self.startCableIn = None
self.switchBlueSelected = None
self.buttonPause = None
self.cmdLEDBlue = 0xFF
self.cmdLEDRed = 0xFF
self.toDisplay = None
self.beep = None
self.compass = None
self.compassRaw = None
self.compassAcc = None
self.compassAccRaw = None
self.sharp = None
self.fnSpeedLimitController = [self.sharpSpeedFn, self.pauseSpeedFn]
self.localisation = None
self.extensions = []
self.dataSources = []
self.modulesForRestart = []
self.can.sendOperationMode()
def error_over_speed(tau, ws):
speeds = np.arange(0.1, 0.9, 0.1)
y = []
for s in speeds:
tg_var = TARGET(speed=s,
max_speed=0.8,
size=20,
peak_cell_num=peak_cell_num)
can = CAN(target=tg_var,
size=20,
tau=tau,
dt=tau / 10,
kappa=0.1,
beta=-8,
ws_param=ws,
h=0)
can.init_network_activity(peak_cell_num=peak_cell_num, init_time=1)
can.run(sim_time=20)
y.append(can.slope_accuracy(s, 20, peak_cell_num))
fig, ax = plt.subplots()
ax.xaxis.set_ticks_position('bottom')
ax.set_title("Error in relation to speed [Tau=" + str(tau) + "][WS=" +
str(ws) + "]")
ax.set_xlabel("Speed [m/s]")
ax.set_ylabel("Error")
plt.plot(speeds, y, 'r-')
plt.ylim(-20, 20)
def driver_self_test(driver, metalog):
assert metalog is not None
can_log_name = metalog.getLog('can')
if metalog.replay:
if metalog.areAssertsEnabled():
can = CAN(ReplayLog(can_log_name), skipInit=True)
else:
can = CAN(ReplayLogInputsOnly(can_log_name), skipInit=True)
else:
can = CAN()
can.relog(can_log_name)
can.resetModules(configFn=setup_faster_update)
robot = JohnDeere(can=can)
robot.UPDATE_TIME_FREQUENCY = 20.0 # TODO change internal and integrate setup
robot.localization = None # TODO
robot.canproxy.stop()
robot.canproxy.set_turn_raw(0)
go_straight(robot, distance=1.0, speed=0.3, with_stop=True)
for i in range(3):
turn(robot, math.radians(-45), radius=2.6, speed=0.5)
turn(robot, math.radians(-45), radius=2.6, speed=-0.5)
robot.canproxy.stop_turn()
robot.wait(3.0)
def __init__(self, can=None, localization=None, config=None):
if can is None:
self.can = CAN()
self.can.resetModules()
else:
self.can = can
self.canproxy = CANProxy(self.can)
self.localization = localization
if config is not None:
global TURN_ANGLE_OFFSET
TURN_ANGLE_OFFSET = config.get('TURN_ANGLE_OFFSET', TURN_ANGLE_OFFSET)
self.time = 0.0
self.buttonGo = None # TODO currently not available (!)
self.drop_ball = False # TODO move to ro.py only
self.extensions = []
self.data_sources = []
self.threads = []
self.modulesForRestart = []
self.can.sendOperationMode()
def __init__(self, can=None):
if can is None:
self.can = CAN()
# self.can.resetModules()
else:
self.can = can
self.time = 0.0
self.status_word = None # not defined yet
self.wheel_angles = [None] * 4 # original CAN data
self.drive_status = [None] * 4
self.zero_steering = None # zero position of all 4 wheels
self.alive = 0 # togle with 128
self.speed_cmd = [0, 0]
self.status_cmd = 3
self.extensions = []
self.data_sources = []
self.threads = []
self.modulesForRestart = []
self.led = 0
self.led_time = 0.0
def single_run():
#tau 0.05, ws 0.08
can = CAN(target=tg,
size=20,
tau=0.05,
dt=0.005,
kappa=0.1,
beta=-8,
ws_param=0.08,
h=0)
can.init_network_activity(peak_cell_num=peak_cell_num, init_time=1)
can.run(sim_time=15)
can.plot_activities(u_out=True)
def __init__(self, can=None):
if can is None:
self.can = CAN()
self.can.resetModules()
else:
self.can = can
self.canproxy = CANProxy(self.can)
self.time = 0.0
self.gas = None
self.steering_angle = 0.0 # in the future None and auto-detect
self.buttonGo = None
self.desired_speed = 0.0
self.filteredGas = None
self.compass = None
self.compassRaw = None
self.compassAcc = None
self.compassAccRaw = None
self.drop_ball = False # TODO move to ro.py only
self.extensions = []
self.data_sources = []
self.modulesForRestart = []
self.can.sendOperationMode()
def testNewSDO(filename):
from can import ReplyLog
can = CAN(ReplyLog(filename))
can.resetModules()
(nodeID, dataIndex, subIndex) = 10, 16 * 256 + 10, 0
reader = ReadSDO(nodeID, dataIndex, subIndex)
for packet in reader.generator():
if packet != None:
can.sendData(*packet)
reader.update(can.readPacket())
print("RESUT DATA:", reader.result)
def multiple_runs(overwrite_file):
x = np.arange(0.01, 0.35, 0.01) #Value range for tau
y = np.arange(0.01, 0.51, 0.01) #Value range for ws_param
delta_t = x / 10
if os.path.isfile(str(speed) +
'-Log.csv') == False or overwrite_file == True:
print("No existing data file found;\nCalculating new data set..")
mat_vals = np.empty((len(y), len(x)))
progress = len(y) * len(x) / 100
counter = 0
for j in range(0, len(x)):
for i in range(0, len(y)):
can = CAN(target=tg,
size=20,
tau=x[j],
dt=delta_t[j],
kappa=0.1,
beta=-8,
ws_param=y[i],
h=0)
can.init_network_activity(peak_cell_num=peak_cell_num,
init_time=1)
can.run(sim_time=20)
result = can.slope_accuracy(speed, 20, peak_cell_num)
mat_vals[i, j] = result
counter += 1
print(counter / progress, "%")
np.savetxt(str(speed) + '-Log.csv', mat_vals, delimiter=',')
else:
print("Loading from existing data file..")
mat_vals = np.loadtxt(str(speed) + '-Log.csv', delimiter=',')
#print(mat_vals)
mat_vals = np.clip(mat_vals, -100, 100)
fig, ax = plt.subplots()
mat = ax.matshow(mat_vals,
origin='lower',
cmap='seismic',
aspect='auto',
extent=(0.01 - 0.01 / 2, 0.34 + 0.01 / 2, 0.01 - 0.01 / 2,
0.5 + 0.01 / 2))
ax.xaxis.set_ticks_position('bottom')
ax.set_title("Error of calculated slope to expected slope for " +
str(speed) + " m/s")
ax.set_xlabel("Tau [s]")
ax.set_ylabel("Pi factor")
ax.set_facecolor('tab:gray')
ax.grid()
cb = plt.colorbar(mat, ax=ax)
cb.set_label("Error value")
def create_robot(metalog, conf):
assert metalog is not None
assert conf is not None # config is required!
can_log_name = metalog.getLog('can')
if metalog.replay:
if metalog.areAssertsEnabled():
can = CAN(ReplayLog(can_log_name), skipInit=True)
else:
can = CAN(ReplayLogInputsOnly(can_log_name), skipInit=True)
else:
can = CAN()
can.relog(can_log_name,
timestamps_log=open(metalog.getLog('timestamps'), 'w'))
can.resetModules(configFn=setup_faster_update)
loc = SimpleOdometry.from_dict(conf.data.get('localization'))
robot = JohnDeere(can=can,
localization=loc,
config=conf.data.get('johndeere'))
robot.UPDATE_TIME_FREQUENCY = 20.0 # TODO change internal and integrate setup
return robot
def self_test(metalog):
assert metalog is not None
can_log_name = metalog.getLog('can')
if metalog.replay:
if metalog.areAssertsEnabled():
can = CAN(ReplayLog(can_log_name), skipInit=True)
else:
can = CAN(ReplayLogInputsOnly(can_log_name), skipInit=True)
else:
can = CAN(speed=CAN.CAN_SPEED_1MB)
can.relog(can_log_name)
#can.resetModules()
robot = Spider3(can=can)
robot.localization = None
try:
robot.extensions.append(('emergency_stop', emergency_stop_extension))
run_it(robot)
except EmergencyStopException:
print("Emergency STOP Exception!")
robot.extensions = []
robot.stop()
robot.wait(3.0)
def __init__(self, can=None):
if can is None:
self.can = CAN()
self.can.resetModules()
else:
self.can = can
self.canproxy = CANProxy(self.can)
self.time = 0.0
self.buttonGo = None # TODO currently not available (!)
self.desired_speed = 0.0
self.drop_ball = False # TODO move to ro.py only
self.extensions = []
self.data_sources = []
self.modulesForRestart = []
self.can.sendOperationMode()
def createRobot(logName, runNumber=0):
metaLog = open(logName)
for line in metaLog:
if line.startswith("CANlog:"):
runNumber -= 1
if runNumber <= 0:
logName = logName[:logName.find("meta")] + line.split(
)[1].split("/")[-1]
break
can = CAN(ReplyLogInputsOnly(logName), skipInit=True)
robot = EduroMaxi(can, replyLog=logName, metaLog=metaLog)
robot.localisation = KalmanFilter() #SimpleOdometry()
robot.attachLaser(remission=True)
robot.attachCamera()
return robot
def testNewSDO( filename ):
from can import ReplyLog
can=CAN(ReplyLog(filename))
can.resetModules()
( nodeID, dataIndex, subIndex ) = 10, 16*256+10, 0
reader = ReadSDO( nodeID, dataIndex, subIndex )
for packet in reader.generator():
if packet != None:
can.sendData( *packet )
reader.update( can.readPacket() )
print("RESUT DATA:", reader.result)
def __init__(self, can=None, localization=None, config=None):
if can is None:
self.can = CAN()
self.can.resetModules()
else:
self.can = can
self.canproxy = CANProxy(self.can)
self.localization = localization
if config is not None:
global TURN_ANGLE_OFFSET
TURN_ANGLE_OFFSET = config.get('TURN_ANGLE_OFFSET', TURN_ANGLE_OFFSET)
self.time = 0.0
self.buttonGo = None # TODO currently not available (!)
self.drop_ball = False # TODO move to ro.py only
self.extensions = []
self.data_sources = []
self.threads = []
self.modulesForRestart = []
self.can.sendOperationMode()
def __init__(self, can=None):
if can is None:
self.can = CAN()
# self.can.resetModules()
else:
self.can = can
self.time = 0.0
self.status_word = None # not defined yet
self.wheel_angles = [None]*4 # original CAN data
self.drive_status = [None]*4
self.zero_steering = None # zero position of all 4 wheels
self.alive = 0 # togle with 128
self.speed_cmd = [0, 0]
self.status_cmd = 3
self.extensions = []
self.data_sources = []
self.threads = []
self.modulesForRestart = []
self.led = 0
self.led_time = 0.0
def create_robot(metalog, conf):
assert metalog is not None
assert conf is not None # config is required!
can_log_name = metalog.getLog('can')
if metalog.replay:
if metalog.areAssertsEnabled():
can = CAN(ReplayLog(can_log_name), skipInit=True)
else:
can = CAN(ReplayLogInputsOnly(can_log_name), skipInit=True)
else:
can = CAN()
can.relog(can_log_name, timestamps_log=open(metalog.getLog('timestamps'), 'w'))
can.resetModules(configFn=setup_faster_update)
loc = SimpleOdometry.from_dict(conf.data.get('localization'))
robot = JohnDeere(can=can, localization=loc, config=conf.data.get('johndeere'))
robot.UPDATE_TIME_FREQUENCY = 20.0 # TODO change internal and integrate setup
return robot
def self_test(metalog):
assert metalog is not None
can_log_name = metalog.getLog('can')
if metalog.replay:
if metalog.areAssertsEnabled():
can = CAN(ReplayLog(can_log_name), skipInit=True)
else:
can = CAN(ReplayLogInputsOnly(can_log_name), skipInit=True)
else:
can = CAN(speed=CAN.CAN_SPEED_1MB)
can.relog(can_log_name)
#can.resetModules()
robot = Spider3(can=can)
robot.localization = None
try:
robot.extensions.append(('emergency_stop', emergency_stop_extension))
run_it(robot)
except EmergencyStopException:
print("Emergency STOP Exception!")
robot.extensions = []
robot.stop()
robot.wait(3.0)
def followme(metalog):
assert metalog is not None
can_log_name = metalog.getLog('can')
if metalog.replay:
if metalog.areAssertsEnabled():
can = CAN(ReplayLog(can_log_name), skipInit=True)
else:
can = CAN(ReplayLogInputsOnly(can_log_name), skipInit=True)
else:
can = CAN()
can.relog(can_log_name)
can.resetModules(configFn=setup_faster_update)
robot = JohnDeere(can=can)
robot.UPDATE_TIME_FREQUENCY = 20.0 # TODO change internal and integrate setup
robot.localization = None # TODO
# GPS
gps_log_name = metalog.getLog('gps')
print gps_log_name
if metalog.replay:
robot.gps = DummySensor()
function = SourceLogger(None, gps_log_name).get
else:
robot.gps = GPS(verbose=0)
function = SourceLogger(robot.gps.coord, gps_log_name).get
robot.gps_data = None
robot.register_data_source('gps', function, gps_data_extension)
# Velodyne
velodyne_log_name = metalog.getLog('velodyne_dist')
print velodyne_log_name
sensor = Velodyne(metalog=metalog)
if metalog.replay:
robot.velodyne = DummySensor()
function = SourceLogger(None, velodyne_log_name).get
else:
robot.velodyne = VelodyneThread(sensor)
function = SourceLogger(robot.velodyne.scan_safe_dist,
velodyne_log_name).get
robot.velodyne_data = None
robot.register_data_source('velodyne', function, velodyne_data_extension)
robot.gps.start() # ASAP so we get GPS fix
robot.velodyne.start(
) # the data source is active, so it is necessary to read-out data
center(robot)
wait_for_start(robot)
moving = False
target_detected = False
robot.desired_speed = 0.5
start_time = robot.time
prev_gps = robot.gps_data
prev_destination_dist = None
while robot.time - start_time < 30 * 60: # limit 30 minutes
robot.update()
if robot.gps_data != prev_gps:
if robot.velodyne_data is not None:
print(robot.velodyne_data[-1], robot.canproxy.wheel_angle_raw,
robot.canproxy.desired_wheel_angle_raw)
prev_gps = robot.gps_data
dist, dist_index = None, None
if robot.velodyne_data is not None:
if len(robot.velodyne_data) == 2:
index, dist = robot.velodyne_data
else:
index, dist, dist_index = robot.velodyne_data
if dist is not None:
dist = min(dist) # currently supported tupple of readings
if dist_index is not None:
target_index, target_dist = dist_index
robot.canproxy.set_turn_raw(int((-100 / 45.) * target_index))
target_detected = target_dist < TARGET_DISTANCE
else:
dist_index = False
if moving:
if dist is None or dist < SAFE_DISTANCE_STOP or not target_detected:
print "!!! STOP !!! -", robot.velodyne_data
robot.canproxy.stop()
moving = False
else: # not moving
if dist is not None and dist > SAFE_DISTANCE_GO:
print "GO", target_detected, robot.velodyne_data
if target_detected:
robot.canproxy.go_slowly()
moving = True
if not robot.buttonGo:
print "STOP!"
break
robot.canproxy.stop_turn()
center(robot)
robot.velodyne.requestStop()
robot.gps.requestStop()
def demo(metalog):
assert metalog is not None
can_log_name = metalog.getLog('can')
if metalog.replay:
if metalog.areAssertsEnabled():
can = CAN(ReplayLog(can_log_name), skipInit=True)
else:
can = CAN(ReplayLogInputsOnly(can_log_name), skipInit=True)
else:
can = CAN()
can.relog(can_log_name)
can.resetModules(configFn=setup_faster_update)
robot = JohnDeere(can=can)
robot.UPDATE_TIME_FREQUENCY = 20.0 # TODO change internal and integrate setup
robot.localization = None # TODO
# GPS
gps_log_name = metalog.getLog('gps')
print(gps_log_name)
if metalog.replay:
robot.gps = DummySensor()
function = SourceLogger(None, gps_log_name).get
else:
robot.gps = GPS(verbose=0)
function = SourceLogger(robot.gps.coord, gps_log_name).get
robot.gps_data = None
robot.register_data_source('gps', function, gps_data_extension)
# Velodyne
velodyne_log_name = metalog.getLog('velodyne_dist')
print(velodyne_log_name)
sensor = Velodyne(metalog=metalog)
if metalog.replay:
robot.velodyne = DummySensor()
function = SourceLogger(None, velodyne_log_name).get
else:
robot.velodyne = VelodyneThread(sensor)
function = SourceLogger(robot.velodyne.scan_safe_dist,
velodyne_log_name).get
robot.velodyne_data = None
robot.register_data_source('velodyne', function, velodyne_data_extension)
robot.gps.start() # ASAP so we get GPS fix
robot.velodyne.start(
) # the data source is active, so it is necessary to read-out data
center(robot)
wait_for_start(robot)
moving = False
robot.desired_speed = 0.5
start_time = robot.time
prev_gps = robot.gps_data
prev_destination_dist = None
while robot.time - start_time < 30 * 60: # limit 30 minutes
robot.update()
if robot.gps_data != prev_gps:
print(robot.time, robot.gas, robot.gps_data, robot.velodyne_data)
prev_gps = robot.gps_data
dist = None
if robot.velodyne_data is not None:
dist_index = None
if len(robot.velodyne_data) == 2:
index, dist = robot.velodyne_data
else:
index, dist, dist_index = robot.velodyne_data
if dist is not None:
dist = min(dist) # currently supported tupple of readings
if moving:
if dist is None or dist < SAFE_DISTANCE_STOP:
print("!!! STOP !!! -", robot.velodyne_data)
#center(robot)
robot.canproxy.stop()
moving = False
elif dist < TURN_DISTANCE:
if abs(robot.steering_angle) < STRAIGHT_EPS:
arr = robot.velodyne_data[1]
num = len(arr)
left, right = min(arr[:num / 2]), min(arr[num / 2:])
print("DECIDE", left, right, robot.velodyne_data)
if left <= right:
robot.canproxy.set_turn_raw(-100)
robot.steering_angle = math.radians(
-30) # TODO replace by autodetect
else:
robot.canproxy.set_turn_raw(100)
robot.steering_angle = math.radians(
30) # TODO replace by autodetect
elif dist > NO_TURN_DISTANCE:
if abs(robot.steering_angle) > STRAIGHT_EPS:
robot.canproxy.set_turn_raw(0)
robot.steering_angle = 0.0 # TODO replace by autodetect
else: # not moving
if dist is not None and dist > SAFE_DISTANCE_GO:
print("GO", robot.velodyne_data)
#go(robot)
robot.canproxy.go()
moving = True
if not robot.buttonGo:
print("STOP!")
break
robot.canproxy.stop_turn()
center(robot)
robot.velodyne.requestStop()
robot.gps.requestStop()
class JohnDeere(object):
UPDATE_TIME_FREQUENCY = 5.0 #20.0 # Hz
def __init__(self, can=None, localization=None, config=None):
if can is None:
self.can = CAN()
self.can.resetModules()
else:
self.can = can
self.canproxy = CANProxy(self.can)
self.localization = localization
if config is not None:
global TURN_ANGLE_OFFSET
TURN_ANGLE_OFFSET = config.get('TURN_ANGLE_OFFSET', TURN_ANGLE_OFFSET)
self.time = 0.0
self.buttonGo = None # TODO currently not available (!)
self.drop_ball = False # TODO move to ro.py only
self.extensions = []
self.data_sources = []
self.threads = []
self.modulesForRestart = []
self.can.sendOperationMode()
def __del__(self):
self.can.sendPreoperationMode()
# TODO to move out, can.py??
def check_modules(self, packet):
# test if all modules are in Operation mode, if not restart them
id, data = packet
if id & 0xFF0 == 0x700: # heart beat message
moduleId = id & 0xF
assert( len(data) == 1 )
if data[0] != 5:
self.can.printPacket( id, data )
if not moduleId in self.modulesForRestart:
self.modulesForRestart.append( moduleId )
print "RESET", moduleId
self.can.sendData( 0, [129,moduleId] )
# if moduleId in [0x01, 0x02]:
# if (0x180 | moduleId) in self.encData:
# # The encoder information is invalid during a reset
# del self.encData[0x180 | moduleId]
elif data[0] == 127: # restarted and in preoperation
print "SWITCH TO OPERATION", moduleId
self.can.sendData( 0, [1,moduleId] )
elif moduleId in self.modulesForRestart:
print "RUNNING", moduleId
self.modulesForRestart.remove(moduleId)
def register_data_source(self, name, function, extension=None):
self.data_sources.append((name, function))
if extension is not None:
self.extensions.append((name, extension))
def send_ball_dispenser(self):
if self.drop_ball:
cmd = 127
else:
cmd = 128
self.can.sendData(0x37F, [0, cmd, 0, 0, 0, 0, 0, 0])
def wait(self, duration):
start_time = self.time
while self.time - start_time < duration:
self.update()
def update(self):
prev_enc = self.canproxy.dist_left_raw, self.canproxy.dist_right_raw
while True:
packet = self.can.readPacket()
self.canproxy.update(packet)
# self.update_emergency_stop(packet)
self.check_modules(packet)
for (name,e) in self.extensions:
e(self, packet[0], packet[1])
# make sure that all updates get also termination SYNC (0x80)
if packet[0] == 0x80:
break
if (self.localization is not None and
prev_enc[0] is not None and prev_enc[1] is not None and
self.canproxy.wheel_angle_raw is not None):
dist_left = ENC_SCALE * (self.canproxy.dist_left_raw - prev_enc[0])
dist_right = ENC_SCALE * (self.canproxy.dist_right_raw - prev_enc[1])
angle_left = self.canproxy.wheel_angle_raw * TURN_SCALE + TURN_ANGLE_OFFSET
self.localization.update_odometry(angle_left, dist_left, dist_right)
# send data related to other sources
for (id,fce) in self.data_sources:
data = fce()
if data != None:
for (name,e) in self.extensions:
e(self, id, data)
self.time += 1.0/self.UPDATE_TIME_FREQUENCY
self.canproxy.set_time(self.time)
self.canproxy.send_speed()
self.canproxy.send_LEDs()
self.send_ball_dispenser()
def set_desired_speed(self, speed):
"""set desired speed in meters per second.
speed = None ... disable speed control
... can be called in every cycle without side-effects
"""
self.canproxy.set_desired_speed_raw(int(speed/ENC_SCALE))
def set_desired_steering(self, angle):
"""set desired steering angle of left wheel"""
# angle = sensors['steering'] * TURN_SCALE + TURN_ANGLE_OFFSET # radians
raw = (angle - TURN_ANGLE_OFFSET)/TURN_SCALE
self.canproxy.set_turn_raw(int(raw))
def stop(self):
"send stop command and make sure robot really stops"
self.canproxy.stop()
for i in xrange(10):
self.update()
def demo(metalog):
assert metalog is not None
can_log_name = metalog.getLog('can')
if metalog.replay:
if metalog.areAssertsEnabled():
can = CAN(ReplayLog(can_log_name), skipInit=True)
else:
can = CAN(ReplayLogInputsOnly(can_log_name), skipInit=True)
else:
can = CAN()
can.relog(can_log_name)
can.resetModules(configFn=setup_faster_update)
robot = JohnDeere(can=can)
robot.UPDATE_TIME_FREQUENCY = 20.0 # TODO change internal and integrate setup
robot.localization = None # TODO
# GPS
gps_log_name = metalog.getLog('gps')
print gps_log_name
if metalog.replay:
robot.gps = DummySensor()
function = SourceLogger(None, gps_log_name).get
else:
robot.gps = GPS(verbose=0)
function = SourceLogger(robot.gps.coord, gps_log_name).get
robot.gps_data = None
robot.register_data_source('gps', function, gps_data_extension)
# Laser
laser_log_name = metalog.getLog('laser')
print laser_log_name
if metalog.replay:
robot.laser = DummySensor()
function = SourceLogger(None, laser_log_name).get
else:
robot.laser = LaserIP()
function = SourceLogger(robot.laser.scan, laser_log_name).get
robot.laser_data = None
robot.register_data_source('laser', function, laser_data_extension)
robot.gps.start() # ASAP so we get GPS fix
robot.laser.start()
print "Wait for gas"
while robot.canproxy.gas is None:
robot.update()
print "Wait for center"
robot.canproxy.stop()
# not available now :( ... wait_for_start(robot)
moving = False
robot.desired_speed = 0.5
start_time = robot.time
prev_gps = robot.gps_data
prev_destination_dist = None
last_laser_update = None
prev_laser = None
while robot.time - start_time < 3*60: # 3min
robot.update()
dist = None
turn_angle = None
if robot.laser_data is not None:
assert len(robot.laser_data) == 541, len(robot.laser_data)
if robot.laser_data != prev_laser:
prev_laser = robot.laser_data
last_laser_update = robot.time
distL, distR = min_dist_arr(robot.laser_data[200:-200])
distL = 20.0 if distL is None else distL
distR = 20.0 if distR is None else distR
dist = min(distL, distR)
turn_angle = follow_wall_angle(robot.laser_data, radius=1.5)
if last_laser_update is not None and robot.time - last_laser_update > 0.3:
print "!!!WARNING!!! Missing laser updates for last {:.1f}s".format(robot.time - last_laser_update)
dist = None # no longer valid distance measurements
if robot.gps_data != prev_gps:
if turn_angle is not None:
print robot.time, robot.canproxy.gas, "(%.3f, %.3f)" % (distL, distR), math.degrees(turn_angle)
else:
print robot.time, robot.canproxy.gas, "(%.3f, %.3f)" % (distL, distR), turn_angle
prev_gps = robot.gps_data
if moving:
if dist is None or dist < SAFE_DISTANCE_STOP:
print "!!! STOP !!!", dist, (distL, distR)
robot.canproxy.stop()
moving = False
else: # not moving
if dist is not None and dist > SAFE_DISTANCE_GO:
print "GO", dist
robot.canproxy.go()
moving = True
if turn_angle is not None:
turn_cmd = max(-120, min(120, 2*math.degrees(turn_angle)))
robot.canproxy.set_turn_raw(turn_cmd)
# print turn_cmd
# if not robot.buttonGo:
# print "STOP!"
# break
robot.canproxy.stop_turn()
robot.canproxy.stop()
for i in xrange(20):
robot.update()
robot.laser.requestStop()
robot.gps.requestStop()
#!/usr/bin/python
def starter(can, configFn=None):
can.sendPreoperationMode()
print "Waiting for release of STOP button"
while 1:
id, data = can.readPacket()
if id == 0x8A:
print data
assert (len(data) == 8)
emergencyStop = (data[0:2] == [0, 0x10]) and (data[3:]
== [0, 0, 0, 0, 0])
print "EmergencyStop:", emergencyStop
if not emergencyStop:
break
return can.resetModules(configFn=configFn)
if __name__ == "__main__":
from can import CAN
can = CAN()
print "Started modules: ", starter(can)
del can
print "Starter terminated"
def __init__(self, can=None, verbose=1):
if can is None:
can = CAN()
# can.resetModules() # event this sometimes does not help :(
self.can = can
self.verbose = verbose
class JohnDeere(object):
UPDATE_TIME_FREQUENCY = 5.0 #20.0 # Hz
def __init__(self, can=None, localization=None, config=None):
if can is None:
self.can = CAN()
self.can.resetModules()
else:
self.can = can
self.canproxy = CANProxy(self.can)
self.localization = localization
if config is not None:
global TURN_ANGLE_OFFSET
TURN_ANGLE_OFFSET = config.get('TURN_ANGLE_OFFSET', TURN_ANGLE_OFFSET)
self.time = 0.0
self.buttonGo = None # TODO currently not available (!)
self.drop_ball = False # TODO move to ro.py only
self.extensions = []
self.data_sources = []
self.threads = []
self.modulesForRestart = []
self.can.sendOperationMode()
def __del__(self):
self.can.sendPreoperationMode()
# TODO to move out, can.py??
def check_modules(self, packet):
# test if all modules are in Operation mode, if not restart them
id, data = packet
if id & 0xFF0 == 0x700: # heart beat message
moduleId = id & 0xF
assert( len(data) == 1 )
if data[0] != 5:
self.can.printPacket( id, data )
if not moduleId in self.modulesForRestart:
self.modulesForRestart.append( moduleId )
print("RESET", moduleId)
self.can.sendData( 0, [129,moduleId] )
# if moduleId in [0x01, 0x02]:
# if (0x180 | moduleId) in self.encData:
# # The encoder information is invalid during a reset
# del self.encData[0x180 | moduleId]
elif data[0] == 127: # restarted and in preoperation
print("SWITCH TO OPERATION", moduleId)
self.can.sendData( 0, [1,moduleId] )
elif moduleId in self.modulesForRestart:
print("RUNNING", moduleId)
self.modulesForRestart.remove(moduleId)
def register_data_source(self, name, function, extension=None):
self.data_sources.append((name, function))
if extension is not None:
self.extensions.append((name, extension))
def send_ball_dispenser(self):
if self.drop_ball:
cmd = 127
else:
cmd = 128
self.can.sendData(0x37F, [0, cmd, 0, 0, 0, 0, 0, 0])
def wait(self, duration):
start_time = self.time
while self.time - start_time < duration:
self.update()
def update(self):
prev_enc = self.canproxy.dist_left_raw, self.canproxy.dist_right_raw
while True:
packet = self.can.readPacket()
self.canproxy.update(packet)
# self.update_emergency_stop(packet)
self.check_modules(packet)
for (name,e) in self.extensions:
e(self, packet[0], packet[1])
# make sure that all updates get also termination SYNC (0x80)
if packet[0] == 0x80:
break
if (self.localization is not None and
prev_enc[0] is not None and prev_enc[1] is not None and
self.canproxy.wheel_angle_raw is not None):
dist_left = ENC_SCALE * (self.canproxy.dist_left_raw - prev_enc[0])
dist_right = ENC_SCALE * (self.canproxy.dist_right_raw - prev_enc[1])
angle_left = self.canproxy.wheel_angle_raw * TURN_SCALE + TURN_ANGLE_OFFSET
self.localization.update_odometry(angle_left, dist_left, dist_right)
# send data related to other sources
for (id,fce) in self.data_sources:
data = fce()
if data != None:
for (name,e) in self.extensions:
e(self, id, data)
self.time += 1.0/self.UPDATE_TIME_FREQUENCY
self.canproxy.set_time(self.time)
self.canproxy.send_speed()
self.canproxy.send_LEDs()
self.send_ball_dispenser()
def set_desired_speed(self, speed):
"""set desired speed in meters per second.
speed = None ... disable speed control
... can be called in every cycle without side-effects
"""
self.canproxy.set_desired_speed_raw(int(speed/ENC_SCALE))
def set_desired_steering(self, angle):
"""set desired steering angle of left wheel"""
# angle = sensors['steering'] * TURN_SCALE + TURN_ANGLE_OFFSET # radians
raw = (angle - TURN_ANGLE_OFFSET)/TURN_SCALE
self.canproxy.set_turn_raw(int(raw))
def stop(self):
"send stop command and make sure robot really stops"
self.canproxy.stop()
for i in range(10):
self.update()
def self_test(metalog):
assert metalog is not None
can_log_name = metalog.getLog('can')
if metalog.replay:
if metalog.areAssertsEnabled():
can = CAN(ReplayLog(can_log_name), skipInit=True)
else:
can = CAN(ReplayLogInputsOnly(can_log_name), skipInit=True)
else:
can = CAN()
can.relog(can_log_name)
can.resetModules(configFn=setup_faster_update)
robot = JohnDeere(can=can)
robot.UPDATE_TIME_FREQUENCY = 20.0 # TODO change internal and integrate setup
robot.canproxy.stop()
# wait_for_start(robot)
robot.desired_speed = 0.5
start_time = robot.time
robot.canproxy.set_turn_raw(0)
robot.canproxy.go()
start_dist = robot.canproxy.dist_left_raw + robot.canproxy.dist_right_raw
arr = []
while robot.time - start_time < 10.0:
robot.update()
arr.append(robot.canproxy.gas)
dist = ENC_SCALE*(robot.canproxy.dist_left_raw + robot.canproxy.dist_right_raw
- start_dist)/2.0
if dist > 1.0:
print "Dist OK at {}s".format(robot.time - start_time), sorted(arr)[len(arr)/2]
break
print dist
robot.stop()
robot.wait(3.0)
dist = ENC_SCALE*(robot.canproxy.dist_left_raw + robot.canproxy.dist_right_raw
- start_dist)/2.0
print dist
print
robot.canproxy.go_back()
start_time = robot.time
start_dist = robot.canproxy.dist_left_raw + robot.canproxy.dist_right_raw
arr = []
while robot.time - start_time < 10.0:
robot.update()
arr.append(robot.canproxy.gas)
dist = ENC_SCALE*(robot.canproxy.dist_left_raw + robot.canproxy.dist_right_raw
- start_dist)/2.0
if dist < -1.0:
print "Dist back OK at {}s".format(robot.time - start_time), sorted(arr)[len(arr)/2]
break
print dist
# print robot.time, robot.canproxy.gas
# if not robot.buttonGo:
# print "STOP!"
# break
robot.canproxy.stop_turn()
robot.stop()
robot.wait(3.0)
dist = ENC_SCALE*(robot.canproxy.dist_left_raw + robot.canproxy.dist_right_raw
- start_dist)/2.0
print dist
def demo(metalog):
assert metalog is not None
can_log_name = metalog.getLog('can')
if metalog.replay:
if metalog.areAssertsEnabled():
can = CAN(ReplayLog(can_log_name), skipInit=True)
else:
can = CAN(ReplayLogInputsOnly(can_log_name), skipInit=True)
else:
can = CAN()
can.relog(can_log_name, timestamps_log=open(metalog.getLog('timestamps'), 'w'))
can.resetModules(configFn=setup_faster_update)
robot = JohnDeere(can=can)
robot.UPDATE_TIME_FREQUENCY = 20.0 # TODO change internal and integrate setup
robot.localization = None # TODO
for sensor_name in ['gps', 'laser', 'camera']:
attach_sensor(robot, sensor_name, metalog)
print("Wait for gas")
while robot.canproxy.gas is None:
robot.update()
print("Wait for center")
robot.canproxy.stop()
# not available now :( ... wait_for_start(robot)
moving = False
robot.desired_speed = 0.5
start_time = robot.time
prev_gps = robot.gps_data
prev_destination_dist = None
last_laser_update = None
prev_laser = None
while robot.time - start_time < 3*60: # 3min
robot.update()
dist = None
turn_angle = None
if robot.laser_data is not None:
assert len(robot.laser_data) == 541, len(robot.laser_data)
if robot.laser_data != prev_laser:
prev_laser = robot.laser_data
last_laser_update = robot.time
distL, distR = min_dist_arr(robot.laser_data[200:-200])
distL = 20.0 if distL is None else distL
distR = 20.0 if distR is None else distR
dist = min(distL, distR)
turn_angle = follow_wall_angle(robot.laser_data, radius=1.5)
if last_laser_update is not None and robot.time - last_laser_update > 0.3:
print("!!!WARNING!!! Missing laser updates for last {:.1f}s".format(robot.time - last_laser_update))
dist = None # no longer valid distance measurements
if robot.gps_data != prev_gps:
if turn_angle is not None:
print(robot.time, robot.canproxy.gas, "(%.3f, %.3f)" % (distL, distR), math.degrees(turn_angle))
else:
print(robot.time, robot.canproxy.gas, "(%.3f, %.3f)" % (distL, distR), turn_angle)
prev_gps = robot.gps_data
if moving:
if dist is None or dist < SAFE_DISTANCE_STOP:
print("!!! STOP !!!", dist, (distL, distR))
robot.canproxy.stop()
moving = False
else: # not moving
if dist is not None and dist > SAFE_DISTANCE_GO:
print("GO", dist)
robot.set_desired_speed(DESIRED_SPEED)
moving = True
if turn_angle is not None:
turn_cmd = max(-120, min(120, 2*math.degrees(turn_angle)))
robot.canproxy.set_turn_raw(turn_cmd)
# print turn_cmd
# if not robot.buttonGo:
# print "STOP!"
# break
robot.canproxy.stop_turn()
robot.canproxy.stop()
for i in range(20):
robot.update()
detach_all_sensors(robot)
def drive_remotely(metalog):
assert metalog is not None
can_log_name = metalog.getLog('can')
if metalog.replay:
if metalog.areAssertsEnabled():
can = CAN(ReplayLog(can_log_name), skipInit=True)
else:
can = CAN(ReplayLogInputsOnly(can_log_name), skipInit=True)
else:
can = CAN()
can.relog(can_log_name)
can.resetModules(configFn=setup_faster_update)
robot = JohnDeere(can=can)
robot.UPDATE_TIME_FREQUENCY = 20.0 # TODO change internal and integrate setup
robot.localization = None # TODO
soc = metalog.createLoggedSocket('remote', headerFormat=None) # TODO fix headerFormat
soc.bind( ('',PORT) )
if not metalog.replay:
soc.soc.setblocking(0)
soc.soc.settimeout( SOCKET_TIMEOUT )
remote_cmd_log_name = metalog.getLog('remote_cmd')
if metalog.replay:
robot.remote = DummySensor()
function = SourceLogger(None, remote_cmd_log_name).get
else:
robot.remote = RemoteThread(soc)
function = SourceLogger(robot.remote.get_data, remote_cmd_log_name).get
max_speed = None
robot.remote_data = None
robot.register_data_source('remote', function, remote_data_extension)
robot.remote.start()
robot.canproxy.stop()
robot.canproxy.set_turn_raw(0)
print "Waiting for remote commands ..."
while robot.remote_data is None:
robot.update()
print "received", robot.remote_data.strip()
moving = False
turning = False
while robot.remote_data != 'END\n':
robot.update()
if robot.remote_data == 'STOP\n' and (moving or turning):
if moving:
robot.canproxy.stop()
print "STOP"
moving = False
if turning:
robot.canproxy.stop_turn()
print "STOP turn"
turning = False
elif robot.remote_data == 'UP\n' and not moving:
if max_speed is None:
robot.canproxy.go()
print "GO"
else:
robot.set_desired_speed(max_speed)
print "GO", max_speed
moving = True
elif robot.remote_data == 'DOWN\n' and not moving:
if max_speed is None:
robot.canproxy.go_back()
print "GO back"
else:
robot.set_desired_speed(-max_speed)
print "GO back", max_speed
moving = True
elif robot.remote_data == 'LEFT\n':
robot.canproxy.set_turn_raw(200)
print "Left"
turning = True
elif robot.remote_data == 'RIGHT\n':
robot.canproxy.set_turn_raw(-200)
print "Right"
turning = True
elif robot.remote_data == 'SPEED0\n':
max_speed = None
elif robot.remote_data.startswith('SPEED'):
max_speed = int(robot.remote_data[len('SPEED'):])/10.0
print "Max Speed", max_speed
print "received", robot.remote_data.strip()
robot.canproxy.stop_turn()
robot.remote.requestStop()
robot.wait(3.0)
def demo(metalog):
assert metalog is not None
can_log_name = metalog.getLog('can')
if metalog.replay:
if metalog.areAssertsEnabled():
can = CAN(ReplayLog(can_log_name), skipInit=True)
else:
can = CAN(ReplayLogInputsOnly(can_log_name), skipInit=True)
else:
can = CAN()
can.relog(can_log_name,
timestamps_log=open(metalog.getLog('timestamps'), 'w'))
can.resetModules(configFn=setup_faster_update)
robot = JohnDeere(can=can)
robot.UPDATE_TIME_FREQUENCY = 20.0 # TODO change internal and integrate setup
robot.localization = None # TODO
for sensor_name in ['gps', 'laser', 'camera']:
attach_sensor(robot, sensor_name, metalog)
print "Wait for gas"
while robot.canproxy.gas is None:
robot.update()
print "Wait for center"
robot.canproxy.stop()
# not available now :( ... wait_for_start(robot)
moving = False
robot.desired_speed = 0.5
start_time = robot.time
prev_gps = robot.gps_data
prev_destination_dist = None
last_laser_update = None
prev_laser = None
while robot.time - start_time < 3 * 60: # 3min
robot.update()
dist = None
turn_angle = None
if robot.laser_data is not None:
assert len(robot.laser_data) == 541, len(robot.laser_data)
if robot.laser_data != prev_laser:
prev_laser = robot.laser_data
last_laser_update = robot.time
distL, distR = min_dist_arr(robot.laser_data[200:-200])
distL = 20.0 if distL is None else distL
distR = 20.0 if distR is None else distR
dist = min(distL, distR)
turn_angle = follow_wall_angle(robot.laser_data, radius=1.5)
if last_laser_update is not None and robot.time - last_laser_update > 0.3:
print "!!!WARNING!!! Missing laser updates for last {:.1f}s".format(
robot.time - last_laser_update)
dist = None # no longer valid distance measurements
if robot.gps_data != prev_gps:
if turn_angle is not None:
print robot.time, robot.canproxy.gas, "(%.3f, %.3f)" % (
distL, distR), math.degrees(turn_angle)
else:
print robot.time, robot.canproxy.gas, "(%.3f, %.3f)" % (
distL, distR), turn_angle
prev_gps = robot.gps_data
if moving:
if dist is None or dist < SAFE_DISTANCE_STOP:
print "!!! STOP !!!", dist, (distL, distR)
robot.canproxy.stop()
moving = False
else: # not moving
if dist is not None and dist > SAFE_DISTANCE_GO:
print "GO", dist
robot.set_desired_speed(DESIRED_SPEED)
moving = True
if turn_angle is not None:
turn_cmd = max(-120, min(120, 2 * math.degrees(turn_angle)))
robot.canproxy.set_turn_raw(turn_cmd)
# print turn_cmd
# if not robot.buttonGo:
# print "STOP!"
# break
robot.canproxy.stop_turn()
robot.canproxy.stop()
for i in xrange(20):
robot.update()
detach_all_sensors(robot)
def robot_go_straight(metalog):
assert metalog is not None
can_log_name = metalog.getLog('can')
if metalog.replay:
if metalog.areAssertsEnabled():
can = CAN(ReplayLog(can_log_name), skipInit=True)
else:
can = CAN(ReplayLogInputsOnly(can_log_name), skipInit=True)
else:
can = CAN()
can.relog(can_log_name, timestamps_log=open(metalog.getLog('timestamps'), 'w'))
can.resetModules(configFn=setup_faster_update)
robot = JohnDeere(can=can, localization=SimpleOdometry())
robot.UPDATE_TIME_FREQUENCY = 20.0 # TODO change internal and integrate setup
for sensor_name in ['gps', 'laser', 'camera']:
attach_sensor(robot, sensor_name, metalog)
robot.canproxy.stop()
robot.set_desired_steering(0.0) # i.e. go straight (!)
try:
robot.extensions.append(('emergency_stop', emergency_stop_extension))
print(robot.canproxy.buttons_and_LEDs)
wait_for_start(robot)
print(robot.canproxy.buttons_and_LEDs)
prev_laser = None
last_laser_update = None
moving = False
dist = None
distL, distR = None, None
prev_camera = None
global_offset = 0.0
while True:
robot.update()
if robot.laser_data is not None:
assert len(robot.laser_data) == 541, len(robot.laser_data)
if robot.laser_data != prev_laser:
prev_laser = robot.laser_data
last_laser_update = robot.time
distL = min_dist(robot.laser_data[:180], INFINITY)
dist = min_dist(robot.laser_data[180:360], INFINITY)
distR = min_dist(robot.laser_data[360:], INFINITY)
print("dist", distL, dist, distR)
if prev_camera != robot.camera_data:
print("CAMERA", robot.camera_data)
prev_camera = robot.camera_data
# filename = 'm:\\git\\osgar\\logs\\pisek170513\\game3\\' + os.path.basename(prev_camera[0])
filename = prev_camera[0]
img = cv2.imread(filename)
if img is not None:
img = img[2*768/3:,:,:]
r = img[:,:,0]
g = img[:,:,1]
b = img[:,:,2]
mask = np.logical_and(g > b, g > r)
img[mask] = 0
left = mask[:, :512].sum()
right = mask[:, 512:].sum()
# print "LEFT_RIGHT", filename, left, right
if left > GREEN_LIMIT or right > GREEN_LIMIT:
if left < right:
global_offset = math.radians(2.5)
else:
global_offset = math.radians(-2.5)
else:
global_offset = 0.0
robot.set_desired_steering(0.0 + global_offset)
if moving:
if dist is None or dist < SAFE_DISTANCE_STOP or min(distL, distR) < SAFE_SIDE_DISTANCE_STOP:
print("!!! STOP !!!", distL, dist, distR)
robot.canproxy.stop()
moving = False
else: # not moving
if dist is not None and dist > SAFE_DISTANCE_GO and min(distL, distR) > SAFE_SIDE_DISTANCE_GO:
print("GO", distL, dist, distR)
robot.set_desired_speed(DESIRED_SPEED)
moving = True
if last_laser_update is not None and robot.time - last_laser_update > 0.3:
print("!!!WARNING!!! Missing laser updates for last {:.1f}s".format(robot.time - last_laser_update))
dist = None # no longer valid distance measurements
except EmergencyStopException:
print("Emergency STOP Exception!")
robot.extensions = [] # hack
robot.canproxy.stop()
robot.canproxy.stop_turn()
robot.wait(3.0)
detach_all_sensors(robot)
class Spider3(object):
UPDATE_TIME_FREQUENCY = 20.0 # Hz
# status word
EMERGENCY_STOP = 0x0001
DEVICE_READY = 0x0002 # also releaseState
MASK_HEARTBEAT = 0x8000
def __init__(self, can=None):
if can is None:
self.can = CAN()
# self.can.resetModules()
else:
self.can = can
self.time = 0.0
self.status_word = None # not defined yet
self.wheel_angles = [None]*4 # original CAN data
self.drive_status = [None]*4
self.zero_steering = None # zero position of all 4 wheels
self.alive = 0 # togle with 128
self.speed_cmd = [0, 0]
self.status_cmd = 3
self.extensions = []
self.data_sources = []
self.threads = []
self.modulesForRestart = []
self.led = 0
self.led_time = 0.0
# self.can.sendOperationMode()
def __del__(self):
pass
# self.can.sendPreoperationMode()
# TODO to move out, can.py??
def check_modules(self, packet):
# test if all modules are in Operation mode, if not restart them
id, data = packet
if id & 0xFF0 == 0x700: # heart beat message
moduleId = id & 0xF
assert( len(data) == 1 )
if data[0] != 5:
self.can.printPacket( id, data )
if not moduleId in self.modulesForRestart:
self.modulesForRestart.append( moduleId )
print("RESET", moduleId)
self.can.sendData( 0, [129,moduleId] )
# if moduleId in [0x01, 0x02]:
# if (0x180 | moduleId) in self.encData:
# # The encoder information is invalid during a reset
# del self.encData[0x180 | moduleId]
elif data[0] == 127: # restarted and in preoperation
print("SWITCH TO OPERATION", moduleId)
self.can.sendData( 0, [1,moduleId] )
elif moduleId in self.modulesForRestart:
print("RUNNING", moduleId)
self.modulesForRestart.remove(moduleId)
def register_data_source(self, name, function, extension=None):
self.data_sources.append((name, function))
if extension is not None:
self.extensions.append((name, extension))
def wait(self, duration):
start_time = self.time
while self.time - start_time < duration:
self.update()
def update_status_word(self, packet):
msg_id, data = packet
if msg_id == 0x200:
assert len(data) == 2, packet
prev = self.status_word
self.status_word = struct.unpack('H', bytearray(data))[0]
if prev is not None:
diff = prev ^ self.status_word
is_alive = diff & self.MASK_HEARTBEAT
if is_alive == 0:
# make sure nothing is missing (for now)
print("\n!!!Spider is DEAD!!! (or data are missing)\n")
if (diff & 0x7FFF) != 0:
print("Status %d -> %d" %
(prev & 0x7FFF, self.status_word & 0x7FFF))
elif msg_id == 0x201:
assert len(data) == 8, packet
prev = self.wheel_angles
self.wheel_angles = struct.unpack('HHHH', bytearray(data))
if prev != self.wheel_angles and self.zero_steering is not None:
# note, that self.zero_steering has 200ms update rate,
# i.e. it does not have to be immediately defined
print('Wheels: %.2f' % self.time, [fix_range(a - b) for a, b in zip(self.wheel_angles, self.zero_steering)])
elif msg_id == 0x202:
assert len(data) == 8, packet # drive status + zero positions
self.drive_status = struct.unpack('HHHH', bytearray(data))
drive = self.drive_status[0] - self.drive_status[2], self.drive_status[3] - self.drive_status[1]
if abs(drive[0]) + abs(drive[1]) > 10:
print('Drive:', drive, abs(drive[0]) + abs(drive[1]))
elif msg_id == 0x203:
assert len(data) == 8, packet
prev = self.zero_steering
self.zero_steering = struct.unpack('HHHH', bytearray(data))
# make sure that calibration did not change during program run
assert prev is None or prev == self.zero_steering, (prev, self.zero_steering)
def send_speed(self):
self.can.sendData(0x400, [self.status_cmd, self.alive])
self.can.sendData(0x401, self.speed_cmd) # drive control data drive, steering
self.alive = 128 - self.alive
def send_LED(self):
self.can.sendData(0x410, [1, 2, 3, 4, 5, 6, 7, self.led])
if self.time - self.led_time > 0.5:
self.led = 1 - self.led
self.led_time = self.time
def set_raw_speed(self, fwd_rev_drive, left_right):
print('set_raw_speed', fwd_rev_drive, left_right)
self.speed_cmd = [fwd_rev_drive, left_right]
def update(self):
while True:
packet = self.can.readPacket()
self.check_modules(packet)
self.update_status_word(packet)
for (name,e) in self.extensions:
e(self, packet[0], packet[1])
# make sure that all updates get also termination SYNC (0x80)
# there is no SYNC on Spider3 yet, use 0x200 for now
if packet[0] == 0x200:
break
# send data related to other sources
for (id,fce) in self.data_sources:
data = fce()
if data != None:
for (name,e) in self.extensions:
e(self, id, data)
self.time += 1.0/self.UPDATE_TIME_FREQUENCY
self.send_speed()
self.send_LED()
def stop(self):
"send stop command and make sure robot really stops"
self.set_raw_speed(0, 0)
for i in range(10):
self.update()
def demo(metalog):
assert metalog is not None
can_log_name = metalog.getLog('can')
if metalog.replay:
if metalog.areAssertsEnabled():
can = CAN(ReplayLog(can_log_name), skipInit=True)
else:
can = CAN(ReplayLogInputsOnly(can_log_name), skipInit=True)
else:
can = CAN()
can.relog(can_log_name)
can.resetModules(configFn=setup_faster_update)
robot = JohnDeere(can=can)
robot.UPDATE_TIME_FREQUENCY = 20.0 # TODO change internal and integrate setup
robot.localization = None # TODO
# GPS
gps_log_name = metalog.getLog('gps')
print gps_log_name
if metalog.replay:
robot.gps = DummySensor()
function = SourceLogger(None, gps_log_name).get
else:
robot.gps = GPS(verbose=0)
function = SourceLogger(robot.gps.coord, gps_log_name).get
robot.gps_data = None
robot.register_data_source('gps', function, gps_data_extension)
# Velodyne
velodyne_log_name = metalog.getLog('velodyne_dist')
print velodyne_log_name
sensor = Velodyne(metalog=metalog)
if metalog.replay:
robot.velodyne = DummySensor()
function = SourceLogger(None, velodyne_log_name).get
else:
robot.velodyne = VelodyneThread(sensor)
function = SourceLogger(robot.velodyne.scan_safe_dist, velodyne_log_name).get
robot.velodyne_data = None
robot.register_data_source('velodyne', function, velodyne_data_extension)
robot.gps.start() # ASAP so we get GPS fix
robot.velodyne.start() # the data source is active, so it is necessary to read-out data
center(robot)
wait_for_start(robot)
moving = False
robot.desired_speed = 0.5
start_time = robot.time
prev_gps = robot.gps_data
prev_destination_dist = None
while robot.time - start_time < 30*60: # limit 30 minutes
robot.update()
if robot.gps_data != prev_gps:
print robot.time, robot.gas, robot.gps_data, robot.velodyne_data
prev_gps = robot.gps_data
dist = None
if robot.velodyne_data is not None:
dist_index = None
if len(robot.velodyne_data) == 2:
index, dist = robot.velodyne_data
else:
index, dist, dist_index = robot.velodyne_data
if dist is not None:
dist = min(dist) # currently supported tupple of readings
if moving:
if dist is None or dist < SAFE_DISTANCE_STOP:
print "!!! STOP !!! -", robot.velodyne_data
#center(robot)
robot.canproxy.stop()
moving = False
elif dist < TURN_DISTANCE:
if abs(robot.steering_angle) < STRAIGHT_EPS:
arr = robot.velodyne_data[1]
num = len(arr)
left, right = min(arr[:num/2]), min(arr[num/2:])
print "DECIDE", left, right, robot.velodyne_data
if left <= right:
robot.canproxy.set_turn_raw(-100)
robot.steering_angle = math.radians(-30) # TODO replace by autodetect
else:
robot.canproxy.set_turn_raw(100)
robot.steering_angle = math.radians(30) # TODO replace by autodetect
elif dist > NO_TURN_DISTANCE:
if abs(robot.steering_angle) > STRAIGHT_EPS:
robot.canproxy.set_turn_raw(0)
robot.steering_angle = 0.0 # TODO replace by autodetect
else: # not moving
if dist is not None and dist > SAFE_DISTANCE_GO:
print "GO", robot.velodyne_data
#go(robot)
robot.canproxy.go()
moving = True
if not robot.buttonGo:
print "STOP!"
break
robot.canproxy.stop_turn()
center(robot)
robot.velodyne.requestStop()
robot.gps.requestStop()
class JohnDeere(object):
UPDATE_TIME_FREQUENCY = 5.0 #20.0 # Hz
def __init__(self, can=None):
if can is None:
self.can = CAN()
self.can.resetModules()
else:
self.can = can
self.canproxy = CANProxy(self.can)
self.time = 0.0
self.gas = None
self.steering_angle = 0.0 # in the future None and auto-detect
self.buttonGo = None
self.desired_speed = 0.0
self.filteredGas = None
self.compass = None
self.compassRaw = None
self.compassAcc = None
self.compassAccRaw = None
self.drop_ball = False # TODO move to ro.py only
self.extensions = []
self.data_sources = []
self.modulesForRestart = []
self.can.sendOperationMode()
def __del__(self):
self.can.sendPreoperationMode()
# TODO to move out, can.py??
def check_modules(self, packet):
# test if all modules are in Operation mode, if not restart them
id, data = packet
if id & 0xFF0 == 0x700: # heart beat message
moduleId = id & 0xF
assert (len(data) == 1)
if data[0] != 5:
self.can.printPacket(id, data)
if not moduleId in self.modulesForRestart:
self.modulesForRestart.append(moduleId)
print "RESET", moduleId
self.can.sendData(0, [129, moduleId])
# if moduleId in [0x01, 0x02]:
# if (0x180 | moduleId) in self.encData:
# # The encoder information is invalid during a reset
# del self.encData[0x180 | moduleId]
elif data[0] == 127: # restarted and in preoperation
print "SWITCH TO OPERATION", moduleId
self.can.sendData(0, [1, moduleId])
elif moduleId in self.modulesForRestart:
print "RUNNING", moduleId
self.modulesForRestart.remove(moduleId)
def register_data_source(self, name, function, extension=None):
self.data_sources.append((name, function))
if extension is not None:
self.extensions.append((name, extension))
def update_gas_status(self, (id, data)):
if id == 0x281:
assert (len(data) >= 8)
self.gas = data[1] * 256 + data[0]
self.buttonGo = (data[-1] > 64)
if self.filteredGas is None:
self.filteredGas = self.gas
else:
self.filteredGas = SCALE_NEW * self.gas + (
1.0 - SCALE_NEW) * self.filteredGas
def ver0(metalog, waypoints=None):
assert metalog is not None
assert waypoints is not None # for simplicity (first is start)
conv = Convertor(refPoint = waypoints[0])
waypoints = waypoints[1:-1] # remove start/finish
can_log_name = metalog.getLog('can')
if metalog.replay:
if metalog.areAssertsEnabled():
can = CAN(ReplayLog(can_log_name), skipInit=True)
else:
can = CAN(ReplayLogInputsOnly(can_log_name), skipInit=True)
else:
can = CAN()
can.relog(can_log_name)
can.resetModules(configFn=setup_faster_update)
robot = JohnDeere(can=can)
robot.UPDATE_TIME_FREQUENCY = 20.0 # TODO change internal and integrate setup
robot.localization = None # TODO
# mount_sensor(GPS, robot, metalog)
gps_log_name = metalog.getLog('gps')
print gps_log_name
if metalog.replay:
robot.gps = DummySensor()
function = SourceLogger(None, gps_log_name).get
else:
robot.gps = GPS(verbose=0)
function = SourceLogger(robot.gps.coord, gps_log_name).get
robot.gps_data = None
robot.register_data_source('gps', function, gps_data_extension)
# mount_sensor(VelodyneThread, robot, metalog)
velodyne_log_name = metalog.getLog('velodyne_dist')
print velodyne_log_name
sensor = Velodyne(metalog=metalog)
if metalog.replay:
robot.velodyne = DummySensor()
function = SourceLogger(None, velodyne_log_name).get
else:
robot.velodyne = VelodyneThread(sensor)
function = SourceLogger(robot.velodyne.scan_safe_dist, velodyne_log_name).get
robot.velodyne_data = None
robot.register_data_source('velodyne', function, velodyne_data_extension)
robot.gps.start() # ASAP so we get GPS fix
robot.velodyne.start() # the data source is active, so it is necessary to read-out data
center(robot)
wait_for_start(robot)
moving = False
robot.desired_speed = 0.5
start_time = robot.time
prev_gps = robot.gps_data
prev_destination_dist = None
while robot.time - start_time < 30*60: # RO timelimit 30 minutes
robot.update()
if robot.gps_data != prev_gps:
print robot.time, robot.gas, robot.gps_data, robot.velodyne_data
prev_gps = robot.gps_data
if robot.gps_data is not None:
dist_arr = [distance( conv.geo2planar((robot.gps_data[1], robot.gps_data[0])),
conv.geo2planar(destination) ) for destination in waypoints]
dist = min(dist_arr)
print "DIST-GPS", dist
if prev_destination_dist is not None:
if prev_destination_dist < dist and dist < 10.0:
robot.drop_ball = True
# remove nearest
i = dist_arr.index(dist) # ugly, but ...
print "INDEX", i
del waypoints[i]
center(robot)
moving = False
robot.wait(1.0)
robot.drop_ball = False
robot.wait(3.0)
dist = None
prev_destination_dist = dist
dist = None
if robot.velodyne_data is not None:
index, dist = robot.velodyne_data
if dist is not None:
dist = min(dist) # currently supported tupple of readings
if moving:
if dist is None or dist < SAFE_DISTANCE_STOP:
print "!!! STOP !!! -", robot.velodyne_data
center(robot)
moving = False
elif dist < TURN_DISTANCE:
if abs(robot.steering_angle) < STRAIGHT_EPS:
arr = robot.velodyne_data[1]
num = len(arr)
left, right = min(arr[:num/2]), min(arr[num/2:])
print "DECIDE", left, right, robot.velodyne_data
if left <= right:
robot.pulse_right(RIGHT_TURN_TIME)
robot.steering_angle = math.radians(-30) # TODO replace by autodetect
else:
robot.pulse_left(LEFT_TURN_TIME)
robot.steering_angle = math.radians(30) # TODO replace by autodetect
elif dist > NO_TURN_DISTANCE:
if abs(robot.steering_angle) > STRAIGHT_EPS:
if robot.steering_angle < 0:
robot.pulse_left(LEFT_TURN_TIME)
else:
robot.pulse_right(RIGHT_TURN_TIME)
robot.steering_angle = 0.0 # TODO replace by autodetect
else: # not moving
if dist is not None and dist > SAFE_DISTANCE_GO:
print "GO", robot.velodyne_data
go(robot)
moving = True
if not robot.buttonGo:
print "STOP!"
break
center(robot)
robot.velodyne.requestStop()
robot.gps.requestStop()
class Spider3(object):
UPDATE_TIME_FREQUENCY = 20.0 # Hz
# status word
EMERGENCY_STOP = 0x0001
DEVICE_READY = 0x0002 # also releaseState
MASK_HEARTBEAT = 0x8000
def __init__(self, can=None):
if can is None:
self.can = CAN()
# self.can.resetModules()
else:
self.can = can
self.time = 0.0
self.status_word = None # not defined yet
self.wheel_angles = [None] * 4 # original CAN data
self.drive_status = [None] * 4
self.zero_steering = None # zero position of all 4 wheels
self.alive = 0 # togle with 128
self.speed_cmd = [0, 0]
self.status_cmd = 3
self.extensions = []
self.data_sources = []
self.threads = []
self.modulesForRestart = []
self.led = 0
self.led_time = 0.0
# self.can.sendOperationMode()
def __del__(self):
pass
# self.can.sendPreoperationMode()
# TODO to move out, can.py??
def check_modules(self, packet):
# test if all modules are in Operation mode, if not restart them
id, data = packet
if id & 0xFF0 == 0x700: # heart beat message
moduleId = id & 0xF
assert (len(data) == 1)
if data[0] != 5:
self.can.printPacket(id, data)
if not moduleId in self.modulesForRestart:
self.modulesForRestart.append(moduleId)
print("RESET", moduleId)
self.can.sendData(0, [129, moduleId])
# if moduleId in [0x01, 0x02]:
# if (0x180 | moduleId) in self.encData:
# # The encoder information is invalid during a reset
# del self.encData[0x180 | moduleId]
elif data[0] == 127: # restarted and in preoperation
print("SWITCH TO OPERATION", moduleId)
self.can.sendData(0, [1, moduleId])
elif moduleId in self.modulesForRestart:
print("RUNNING", moduleId)
self.modulesForRestart.remove(moduleId)
def register_data_source(self, name, function, extension=None):
self.data_sources.append((name, function))
if extension is not None:
self.extensions.append((name, extension))
def wait(self, duration):
start_time = self.time
while self.time - start_time < duration:
self.update()
def update_status_word(self, packet):
msg_id, data = packet
if msg_id == 0x200:
assert len(data) == 2, packet
prev = self.status_word
self.status_word = struct.unpack('H', bytearray(data))[0]
if prev is not None:
diff = prev ^ self.status_word
is_alive = diff & self.MASK_HEARTBEAT
if is_alive == 0:
# make sure nothing is missing (for now)
print("\n!!!Spider is DEAD!!! (or data are missing)\n")
if (diff & 0x7FFF) != 0:
print("Status %d -> %d" %
(prev & 0x7FFF, self.status_word & 0x7FFF))
elif msg_id == 0x201:
assert len(data) == 8, packet
prev = self.wheel_angles
self.wheel_angles = struct.unpack('HHHH', bytearray(data))
if prev != self.wheel_angles and self.zero_steering is not None:
# note, that self.zero_steering has 200ms update rate,
# i.e. it does not have to be immediately defined
print('Wheels: %.2f' % self.time, [
fix_range(a - b)
for a, b in zip(self.wheel_angles, self.zero_steering)
])
elif msg_id == 0x202:
assert len(data) == 8, packet # drive status + zero positions
self.drive_status = struct.unpack('HHHH', bytearray(data))
drive = self.drive_status[0] - self.drive_status[
2], self.drive_status[3] - self.drive_status[1]
if abs(drive[0]) + abs(drive[1]) > 10:
print('Drive:', drive, abs(drive[0]) + abs(drive[1]))
elif msg_id == 0x203:
assert len(data) == 8, packet
prev = self.zero_steering
self.zero_steering = struct.unpack('HHHH', bytearray(data))
# make sure that calibration did not change during program run
assert prev is None or prev == self.zero_steering, (
prev, self.zero_steering)
def send_speed(self):
self.can.sendData(0x400, [self.status_cmd, self.alive])
self.can.sendData(0x401,
self.speed_cmd) # drive control data drive, steering
self.alive = 128 - self.alive
def send_LED(self):
self.can.sendData(0x410, [1, 2, 3, 4, 5, 6, 7, self.led])
if self.time - self.led_time > 0.5:
self.led = 1 - self.led
self.led_time = self.time
def set_raw_speed(self, fwd_rev_drive, left_right):
print('set_raw_speed', fwd_rev_drive, left_right)
self.speed_cmd = [fwd_rev_drive, left_right]
def update(self):
while True:
packet = self.can.readPacket()
self.check_modules(packet)
self.update_status_word(packet)
for (name, e) in self.extensions:
e(self, packet[0], packet[1])
# make sure that all updates get also termination SYNC (0x80)
# there is no SYNC on Spider3 yet, use 0x200 for now
if packet[0] == 0x200:
break
# send data related to other sources
for (id, fce) in self.data_sources:
data = fce()
if data != None:
for (name, e) in self.extensions:
e(self, id, data)
self.time += 1.0 / self.UPDATE_TIME_FREQUENCY
self.send_speed()
self.send_LED()
def stop(self):
"send stop command and make sure robot really stops"
self.set_raw_speed(0, 0)
for i in range(10):
self.update()
def launch(cmd_args, robotFactory, task, configFn = None, canArgs={}):
'''
Launches a robot to the given file.
Parameters:
cmd_args ... List of parameters in the "configFile [--file logfile [F|FF]]" form.
robotFactory ... Function (or class) which can be called with the following named
parameters and creates an instance of the robot:
robot = robotFactory(can=..., replyLog=...)
task ... Factory which called as "t = task(robot, configFileName, verbose=...)"
creates a runable instance. Ie. "t()" runs the show.
configFn ... A method called in the robot's preconfigration mode.
canArgs ... Named arguments passed down to the CAN.
'''
if len(cmd_args) < 2:
print __doc__
sys.exit(-1)
configFilename = cmd_args[1]
logName = None
simulation = None
metaLog = None
if len(cmd_args) > 2:
if cmd_args[2]=='--file':
logName = cmd_args[3]
if "meta" in logName:
runNumber = 1
try:
if 'F' == cmd_args[-1] or 'FF' == cmd_args[-1]:
runNumber = int(cmd_args[-2])
else:
runNumber = int(cmd_args[-1])
except:
pass
print "METALOG", logName, runNumber
metaLog = open(logName)
for line in metaLog:
if line.startswith("CANlog:"):
runNumber -= 1
if runNumber <= 0:
logName = logName[:logName.find("meta")]+line.split()[1].split("/")[-1]
break
if len(cmd_args)> 4 and cmd_args[-1]=='FF':
can = CAN( ReplyLogInputsOnly( logName ), skipInit = True )
else:
assertWrite = not (len(cmd_args) > 4 and cmd_args[-1]=='F')
can = CAN( ReplyLog( logName, assertWrite=assertWrite ), skipInit = True )
else:
assert( cmd_args[2]=='--simul' )
simulation = True # TODO
else:
metaLog = open( timeName("logs/meta", "log"), "w" )
metaLog.write( str(cmd_args) + "\n" )
metaLog.write( "configFilename: '" + str(configFilename) + "'\n" )
if configFilename:
try:
metaLog.write( "------------------\n" )
metaLog.write( open(configFilename).read() )
metaLog.write( "------------------\n" )
except:
metaLog.write( "Load FAILED\n" )
metaLog.flush()
can = CAN(**canArgs)
isFromLog = logName is not None
while 1:
if not isFromLog and not simulation:
can.relog('logs/s')
print starter.starter( can, configFn=configFn )
metaLog.write( "CANlog:\t" + str(can.relog('logs/r')) + "\n" )
metaLog.flush()
if simulation:
robot = Simulator()
else:
robot = robotFactory(can=can, replyLog=logName, metaLog=metaLog)
if isFromLog or simulation:
viewlog.viewLogFile = open( "view.log", "w" )
robot.addExtension( viewLogExtension )
# robot.addExtension( viewCompassExtension )
robot.addExtension( viewPoseExtension )
laserViewer = LaserViewer( robot, distanceLimit = 2.0, sideLimit = 10 )
if isFromLog:
view = ViewCameraExtension( os.path.dirname( sys.argv[3] ) )
robot.addExtension( view.viewCameraExtension )
if not __run( robot, task, configFilename, verbose=(isFromLog or simulation) ) or isFromLog or simulation:
break
del robot # robot has switch to preoperation ... probably not a good idea
def launch(cmd_args, robotFactory, task, configFn=None, canArgs={}):
'''
Launches a robot to the given file.
Parameters:
cmd_args ... List of parameters in the "configFile [--file logfile [F|FF]]" form.
robotFactory ... Function (or class) which can be called with the following named
parameters and creates an instance of the robot:
robot = robotFactory(can=..., replyLog=...)
task ... Factory which called as "t = task(robot, configFileName, verbose=...)"
creates a runable instance. Ie. "t()" runs the show.
configFn ... A method called in the robot's preconfigration mode.
canArgs ... Named arguments passed down to the CAN.
'''
if len(cmd_args) < 2:
print __doc__
sys.exit(-1)
configFilename = cmd_args[1]
logName = None
simulation = None
metaLog = None
if len(cmd_args) > 2:
if cmd_args[2] == '--file':
logName = cmd_args[3]
if "meta" in logName:
runNumber = 1
try:
if 'F' == cmd_args[-1] or 'FF' == cmd_args[-1]:
runNumber = int(cmd_args[-2])
else:
runNumber = int(cmd_args[-1])
except:
pass
print "METALOG", logName, runNumber
metaLog = open(logName)
for line in metaLog:
if line.startswith("CANlog:"):
runNumber -= 1
if runNumber <= 0:
logName = logName[:logName.find(
"meta")] + line.split()[1].split("/")[-1]
break
if len(cmd_args) > 4 and cmd_args[-1] == 'FF':
can = CAN(ReplyLogInputsOnly(logName), skipInit=True)
else:
assertWrite = not (len(cmd_args) > 4 and cmd_args[-1] == 'F')
can = CAN(ReplyLog(logName, assertWrite=assertWrite),
skipInit=True)
else:
assert (cmd_args[2] == '--simul')
simulation = True # TODO
else:
metaLog = open(timeName("logs/meta", "log"), "w")
metaLog.write(str(cmd_args) + "\n")
metaLog.write("configFilename: '" + str(configFilename) + "'\n")
if configFilename:
try:
metaLog.write("------------------\n")
metaLog.write(open(configFilename).read())
metaLog.write("------------------\n")
except:
metaLog.write("Load FAILED\n")
metaLog.flush()
can = CAN(**canArgs)
isFromLog = logName is not None
while 1:
if not isFromLog and not simulation:
can.relog('logs/s')
print starter.starter(can, configFn=configFn)
metaLog.write("CANlog:\t" + str(can.relog('logs/r')) + "\n")
metaLog.flush()
if simulation:
robot = Simulator()
else:
robot = robotFactory(can=can, replyLog=logName, metaLog=metaLog)
if isFromLog or simulation:
viewlog.viewLogFile = open("view.log", "w")
robot.addExtension(viewLogExtension)
# robot.addExtension( viewCompassExtension )
robot.addExtension(viewPoseExtension)
laserViewer = LaserViewer(robot, distanceLimit=2.0, sideLimit=10)
if isFromLog:
view = ViewCameraExtension(os.path.dirname(sys.argv[3]))
robot.addExtension(view.viewCameraExtension)
if not __run(
robot, task, configFilename,
verbose=(isFromLog or simulation)) or isFromLog or simulation:
break
del robot # robot has switch to preoperation ... probably not a good idea
def drive_remotely(metalog):
assert metalog is not None
can_log_name = metalog.getLog('can')
if metalog.replay:
if metalog.areAssertsEnabled():
can = CAN(ReplayLog(can_log_name), skipInit=True)
else:
can = CAN(ReplayLogInputsOnly(can_log_name), skipInit=True)
else:
can = CAN()
can.relog(can_log_name)
can.resetModules(configFn=setup_faster_update)
robot = JohnDeere(can=can)
robot.UPDATE_TIME_FREQUENCY = 20.0 # TODO change internal and integrate setup
robot.localization = None # TODO
soc = metalog.createLoggedSocket('remote', headerFormat=None) # TODO fix headerFormat
soc.bind( ('',PORT) )
if not metalog.replay:
soc.soc.setblocking(0)
soc.soc.settimeout( SOCKET_TIMEOUT )
remote_cmd_log_name = metalog.getLog('remote_cmd')
if metalog.replay:
robot.remote = DummySensor()
function = SourceLogger(None, remote_cmd_log_name).get
else:
robot.remote = RemoteThread(soc)
function = SourceLogger(robot.remote.get_data, remote_cmd_log_name).get
max_speed = None
robot.remote_data = None
robot.register_data_source('remote', function, remote_data_extension)
robot.remote.start()
robot.canproxy.stop()
robot.canproxy.set_turn_raw(0)
print("Waiting for remote commands ...")
while robot.remote_data is None:
robot.update()
print("received", robot.remote_data.strip())
moving = False
turning = False
while robot.remote_data != 'END\n':
robot.update()
if robot.remote_data == 'STOP\n' and (moving or turning):
if moving:
robot.canproxy.stop()
print("STOP")
moving = False
if turning:
robot.canproxy.stop_turn()
print("STOP turn")
turning = False
elif robot.remote_data == 'UP\n' and not moving:
if max_speed is None:
robot.canproxy.go()
print("GO")
else:
robot.set_desired_speed(max_speed)
print("GO", max_speed)
moving = True
elif robot.remote_data == 'DOWN\n' and not moving:
if max_speed is None:
robot.canproxy.go_back()
print("GO back")
else:
robot.set_desired_speed(-max_speed)
print("GO back", max_speed)
moving = True
elif robot.remote_data == 'LEFT\n':
robot.canproxy.set_turn_raw(200)
print("Left")
turning = True
elif robot.remote_data == 'RIGHT\n':
robot.canproxy.set_turn_raw(-200)
print("Right")
turning = True
elif robot.remote_data == 'SPEED0\n':
max_speed = None
elif robot.remote_data.startswith('SPEED'):
max_speed = int(robot.remote_data[len('SPEED'):])/10.0
print("Max Speed", max_speed)
print("received", robot.remote_data.strip())
robot.canproxy.stop_turn()
robot.remote.requestStop()
robot.wait(3.0)
class JohnDeere(object):
UPDATE_TIME_FREQUENCY = 5.0 #20.0 # Hz
def __init__(self, can=None):
if can is None:
self.can = CAN()
self.can.resetModules()
else:
self.can = can
self.canproxy = CANProxy(self.can)
self.time = 0.0
self.buttonGo = None # TODO currently not available (!)
self.desired_speed = 0.0
self.drop_ball = False # TODO move to ro.py only
self.extensions = []
self.data_sources = []
self.modulesForRestart = []
self.can.sendOperationMode()
def __del__(self):
self.can.sendPreoperationMode()
# TODO to move out, can.py??
def check_modules(self, packet):
# test if all modules are in Operation mode, if not restart them
id, data = packet
if id & 0xFF0 == 0x700: # heart beat message
moduleId = id & 0xF
assert( len(data) == 1 )
if data[0] != 5:
self.can.printPacket( id, data )
if not moduleId in self.modulesForRestart:
self.modulesForRestart.append( moduleId )
print "RESET", moduleId
self.can.sendData( 0, [129,moduleId] )
# if moduleId in [0x01, 0x02]:
# if (0x180 | moduleId) in self.encData:
# # The encoder information is invalid during a reset
# del self.encData[0x180 | moduleId]
elif data[0] == 127: # restarted and in preoperation
print "SWITCH TO OPERATION", moduleId
self.can.sendData( 0, [1,moduleId] )
elif moduleId in self.modulesForRestart:
print "RUNNING", moduleId
self.modulesForRestart.remove(moduleId)
def register_data_source(self, name, function, extension=None):
self.data_sources.append((name, function))
if extension is not None:
self.extensions.append((name, extension))
def send_ball_dispenser(self):
if self.drop_ball:
cmd = 127
else:
cmd = 128
self.can.sendData(0x37F, [0, cmd, 0, 0, 0, 0, 0, 0])
def wait(self, duration):
start_time = self.time
while self.time - start_time < duration:
self.update()
def update(self):
while True:
packet = self.can.readPacket()
self.canproxy.update(packet)
# self.update_emergency_stop(packet)
self.check_modules(packet)
for (name,e) in self.extensions:
e(self, packet[0], packet[1])
# make sure that all updates get also termination SYNC (0x80)
if packet[0] == 0x80:
break
# send data related to other sources
for (id,fce) in self.data_sources:
data = fce()
if data != None:
for (name,e) in self.extensions:
e(self, id, data)
self.time += 1.0/self.UPDATE_TIME_FREQUENCY
self.canproxy.set_time(self.time)
self.canproxy.send_speed()
self.send_ball_dispenser()
def stop(self):
"send stop command and make sure robot really stops"
self.desired_speed = 0.0
self.canproxy.stop()
for i in xrange(10):
self.update()
def demo(metalog):
assert metalog is not None
can_log_name = metalog.getLog('can')
if metalog.replay:
if metalog.areAssertsEnabled():
can = CAN(ReplayLog(can_log_name), skipInit=True)
else:
can = CAN(ReplayLogInputsOnly(can_log_name), skipInit=True)
else:
can = CAN()
can.relog(can_log_name)
can.resetModules(configFn=setup_faster_update)
robot = JohnDeere(can=can)
robot.UPDATE_TIME_FREQUENCY = 20.0 # TODO change internal and integrate setup
robot.localization = None # TODO
# GPS
gps_log_name = metalog.getLog('gps')
print gps_log_name
if metalog.replay:
robot.gps = DummySensor()
function = SourceLogger(None, gps_log_name).get
else:
robot.gps = GPS(verbose=0)
function = SourceLogger(robot.gps.coord, gps_log_name).get
robot.gps_data = None
robot.register_data_source('gps', function, gps_data_extension)
# Laser
laser_log_name = metalog.getLog('laser')
print laser_log_name
if metalog.replay:
robot.laser = DummySensor()
function = SourceLogger(None, laser_log_name).get
else:
robot.laser = LaserIP()
function = SourceLogger(robot.laser.scan, laser_log_name).get
robot.laser_data = None
robot.register_data_source('laser', function, laser_data_extension)
robot.gps.start() # ASAP so we get GPS fix
robot.laser.start()
while robot.gas is None:
robot.update()
center(robot)
wait_for_start(robot)
moving = False
robot.desired_speed = 0.5
start_time = robot.time
prev_gps = robot.gps_data
prev_destination_dist = None
while robot.time - start_time < 30*60: # limit 30 minutes
robot.update()
dist = None
if robot.laser_data is not None:
assert len(robot.laser_data) == 541, len(robot.laser_data)
distL, distR = min_dist_arr(robot.laser_data[200:-200])
distL = 20.0 if distL is None else distL
distR = 20.0 if distR is None else distR
dist = min(distL, distR)
if robot.gps_data != prev_gps:
print robot.time, robot.gas, robot.gps_data, (distL, distR)
prev_gps = robot.gps_data
if moving:
if dist is None or dist < SAFE_DISTANCE_STOP:
print "!!! STOP !!!", dist, (distL, distR)
robot.canproxy.stop()
moving = False
elif dist < TURN_DISTANCE:
if abs(robot.steering_angle) < STRAIGHT_EPS:
print "DECIDE", distL, distR
if distL <= distR:
robot.canproxy.set_turn_raw(-100)
robot.steering_angle = math.radians(-30)
else:
robot.canproxy.set_turn_raw(100)
robot.steering_angle = math.radians(30)
elif dist > NO_TURN_DISTANCE:
if abs(robot.steering_angle) > STRAIGHT_EPS:
robot.canproxy.set_turn_raw(0)
robot.steering_angle = 0.0
else: # not moving
if dist is not None and dist > SAFE_DISTANCE_GO:
print "GO", dist
robot.canproxy.go()
moving = True
if not robot.buttonGo:
print "STOP!"
break
robot.canproxy.stop_turn()
center(robot)
robot.laser.requestStop()
robot.gps.requestStop()
def followme(metalog):
assert metalog is not None
can_log_name = metalog.getLog('can')
if metalog.replay:
if metalog.areAssertsEnabled():
can = CAN(ReplayLog(can_log_name), skipInit=True)
else:
can = CAN(ReplayLogInputsOnly(can_log_name), skipInit=True)
else:
can = CAN()
can.relog(can_log_name)
can.resetModules(configFn=setup_faster_update)
robot = JohnDeere(can=can)
robot.UPDATE_TIME_FREQUENCY = 20.0 # TODO change internal and integrate setup
robot.localization = None # TODO
# GPS
gps_log_name = metalog.getLog('gps')
print(gps_log_name)
if metalog.replay:
robot.gps = DummySensor()
function = SourceLogger(None, gps_log_name).get
else:
robot.gps = GPS(verbose=0)
function = SourceLogger(robot.gps.coord, gps_log_name).get
robot.gps_data = None
robot.register_data_source('gps', function, gps_data_extension)
# Velodyne
velodyne_log_name = metalog.getLog('velodyne_dist')
print(velodyne_log_name)
sensor = Velodyne(metalog=metalog)
if metalog.replay:
robot.velodyne = DummySensor()
function = SourceLogger(None, velodyne_log_name).get
else:
robot.velodyne = VelodyneThread(sensor)
function = SourceLogger(robot.velodyne.scan_safe_dist, velodyne_log_name).get
robot.velodyne_data = None
robot.register_data_source('velodyne', function, velodyne_data_extension)
robot.gps.start() # ASAP so we get GPS fix
robot.velodyne.start() # the data source is active, so it is necessary to read-out data
center(robot)
wait_for_start(robot)
moving = False
target_detected = False
robot.desired_speed = 0.5
start_time = robot.time
prev_gps = robot.gps_data
prev_destination_dist = None
while robot.time - start_time < 30*60: # limit 30 minutes
robot.update()
if robot.gps_data != prev_gps:
if robot.velodyne_data is not None:
print((robot.velodyne_data[-1], robot.canproxy.wheel_angle_raw,
robot.canproxy.desired_wheel_angle_raw))
prev_gps = robot.gps_data
dist, dist_index = None, None
if robot.velodyne_data is not None:
if len(robot.velodyne_data) == 2:
index, dist = robot.velodyne_data
else:
index, dist, dist_index = robot.velodyne_data
if dist is not None:
dist = min(dist) # currently supported tupple of readings
if dist_index is not None:
target_index, target_dist = dist_index
robot.canproxy.set_turn_raw(int((-100/45.)*target_index))
target_detected = target_dist < TARGET_DISTANCE
else:
dist_index = False
if moving:
if dist is None or dist < SAFE_DISTANCE_STOP or not target_detected:
print("!!! STOP !!! -", robot.velodyne_data)
robot.canproxy.stop()
moving = False
else: # not moving
if dist is not None and dist > SAFE_DISTANCE_GO:
print("GO", target_detected, robot.velodyne_data)
if target_detected:
robot.canproxy.go_slowly()
moving = True
if not robot.buttonGo:
print("STOP!")
break
robot.canproxy.stop_turn()
center(robot)
robot.velodyne.requestStop()
robot.gps.requestStop()
def self_test(metalog):
assert metalog is not None
can_log_name = metalog.getLog('can')
if metalog.replay:
if metalog.areAssertsEnabled():
can = CAN(ReplayLog(can_log_name), skipInit=True)
else:
can = CAN(ReplayLogInputsOnly(can_log_name), skipInit=True)
else:
can = CAN()
can.relog(can_log_name)
can.resetModules(configFn=setup_faster_update)
robot = JohnDeere(can=can)
robot.UPDATE_TIME_FREQUENCY = 20.0 # TODO change internal and integrate setup
robot.canproxy.stop()
# wait_for_start(robot)
robot.set_desired_speed(0.5)
start_time = robot.time
robot.canproxy.set_turn_raw(0)
robot.canproxy.go()
start_dist = robot.canproxy.dist_left_raw + robot.canproxy.dist_right_raw
arr = []
while robot.time - start_time < 10.0:
robot.update()
arr.append(robot.canproxy.gas)
dist = ENC_SCALE*(robot.canproxy.dist_left_raw + robot.canproxy.dist_right_raw
- start_dist)/2.0
if dist > 1.0:
print "Dist OK at {}s".format(robot.time - start_time), sorted(arr)[len(arr)/2]
break
print dist
robot.stop()
robot.wait(3.0)
dist = ENC_SCALE*(robot.canproxy.dist_left_raw + robot.canproxy.dist_right_raw
- start_dist)/2.0
print dist
print
robot.canproxy.go_back()
start_time = robot.time
start_dist = robot.canproxy.dist_left_raw + robot.canproxy.dist_right_raw
arr = []
while robot.time - start_time < 10.0:
robot.update()
arr.append(robot.canproxy.gas)
dist = ENC_SCALE*(robot.canproxy.dist_left_raw + robot.canproxy.dist_right_raw
- start_dist)/2.0
if dist < -1.0:
print "Dist back OK at {}s".format(robot.time - start_time), sorted(arr)[len(arr)/2]
break
print dist
# print robot.time, robot.canproxy.gas
# if not robot.buttonGo:
# print "STOP!"
# break
robot.canproxy.stop_turn()
robot.stop()
robot.wait(3.0)
dist = ENC_SCALE*(robot.canproxy.dist_left_raw + robot.canproxy.dist_right_raw
- start_dist)/2.0
print dist