#declare variables
linear_error = LIN_ERROR_THRESHOLD + 1
prev_linear_error = linear_error
angular_error = ANG_ERROR_THRESHOLD + 1
prev_angular_error = angular_error
prev_fix_time = 0
need_tgt = True
fix_timeout = False
linearPID = PID(L_P, L_I, L_D)
angularPID = PID(A_P, A_I, A_D)
gps = GPS('COM5', 9600, 1)
""" -MAIN LOOP- """
def talker():
global need_tgt, fix_timeout
global prev_fix_time, prev_linear_error, prev_angular_error
#create ROS publisher
pub = rospy.Publisher("/Drive_Train", String)
rospy.init_node("Autonomous_Waypoint_Navigation")
#open serial port and turn on GPS power
#gps.begin()
while not rospy.is_shutdown():
def main():
window.addToParent()
window.setBackground('white')
window.clear()
window.resetView()
secondary_window.addToParent()
secondary_window.setBackground('white')
secondary_window.clear()
road_info_window.setBackground('white')
road_info_window.clear()
config_data = main_menu.config_data
map_data = config_data["map_data"]
S = map_data["coords_south"]
W = map_data["coords_west"]
N = map_data["coords_north"]
E = map_data["coords_east"]
llc = LatLonConverter(window, S, W, N, E)
graph = Graph()
graph.load_open_street_map_data(map_data["filename"], llc)
road_map = RoadMap(graph, window)
road_map.draw()
road_map.draw_road_names()
gps = GPS(graph, road_map)
cars = []
car_shapes = []
car_factory = CarFactory(window, gps, cars, car_shapes)
num_cars = config_data["num_cars"]
for _ in range(num_cars):
car_factory.create()
# collision_system = GridCollisionSystem(window, cars)
collision_system = QuadTreeCollisionSystem(window, cars)
info = InfoWindow(secondary_window)
info.set_selected_car(cars[0])
info.initialize_table()
car_shapes[info.selected_car.index].shape.setFill("yellow")
road_info = RoadInfoWindow(road_info_window)
for car_shape in car_shapes:
car_shape.draw()
# initialize simulation variables
simTime = 0.0
limit = 10000
TICKS_PER_SECOND = 30
TIME_PER_TICK = 1.0 / TICKS_PER_SECOND
nextLogicTick = TIME_PER_TICK
lastFrameTime = time.time()
lag = 0.0
# Main Simulation Loop
while simTime < limit:
currentTime = time.time()
elapsed = currentTime - lastFrameTime
lastFrameTime = currentTime
lag += elapsed
simTime += elapsed
# process events
window.update()
secondary_window.update()
road_info_window.update()
frame.update()
last_pressed_key = (window.checkKey() or secondary_window.checkKey()
or road_info_window.checkKey())
if last_pressed_key is not None:
if last_pressed_key == "space":
pause()
lastFrameTime = time.time()
elif last_pressed_key == "p":
window.zoomIn()
elif last_pressed_key == "o":
window.zoomOut()
elif last_pressed_key == "d":
print(road_map.get_roads_within_view())
last_clicked_pt = window.checkMouse()
if last_clicked_pt is not None:
car_clicked = False
map_obj_clicked = False
for car_shape in car_shapes:
if car_shape.clicked(last_clicked_pt):
car_shapes[info.selected_car.index].shape.setFill("white")
info.set_selected_car(cars[car_shape.index])
car_shapes[info.selected_car.index].shape.setFill("yellow")
car_clicked = True
break
if not car_clicked:
nearby_object_ids = road_map.get_nearby_object_ids(
last_clicked_pt.x, last_clicked_pt.y)
for map_obj_id in nearby_object_ids:
map_obj = road_map.get_obj_by_id(map_obj_id)
if map_obj.clicked(last_clicked_pt):
relx, rely = window.getRelativeScreenPos(
last_clicked_pt.x, last_clicked_pt.y)
road_info_window_options = {
"place": {
"relx": relx,
"rely": rely
}
}
road_info_window.addToParent(road_info_window_options)
road_info.set_selected_item(map_obj)
map_obj_clicked = True
break
if not map_obj_clicked:
road_info_window.forget()
last_clicked_pt = secondary_window.checkMouse()
if last_clicked_pt is not None:
secondary_window.update()
for button in info.buttons:
button.clicked(last_clicked_pt)
continue
# update simulation logic
while lag > TIME_PER_TICK:
collision_system.process_collisions(cars)
for car in cars:
car.move_towards_dest(TIME_PER_TICK)
car_shape = car_shapes[car.index]
car_shape.x = cars[car.index].x
car_shape.y = cars[car.index].y
collision_system.update_objects(cars)
nextLogicTick += TIME_PER_TICK
lag -= TIME_PER_TICK
# render updates to window
for car_shape in car_shapes:
car_shape.render()
info.update_table()
if info.follow_car:
window.centerScreenOnPoint(info.selected_car.x,
info.selected_car.y)
road_info.update_table()
road_map.draw_route(info.selected_car, info.show_route)
_root.update_idletasks()
cleanup()
prev_z = 1000
if abs((prev_z - zAccl) / zAccl) >= 1.0: ### Change this
bus.write_byte_data(0x1D, 0x2A, 0)
# record GPS for 1 second.
with open("/home/pi/Rpi/data/shots.csv" % shot, "a+") as writefile:
print("Trigger")
led.turn_on()
writer = csv.writer(writefile)
if shot == 0:
writer.writerow([
"Shot", "Time", "Lat", "Lat Err", "Lon", "Lon Err", "Alt",
"Alt Err"
])
start = time.time()
while time.time() - start < 1:
data = GPS()
data = np.concatenate([[shot], data])
writer.writerow(data)
time.sleep(0.2)
writefile.flush()
writefile.close()
shot += 1
time.sleep(30)
led.turn_off()
prev_z = 1000
status = True
continue
status = False
prev_z = zAccl
def main(controller_type,
training_data,
policy_file,
gps_params=gps_params_default):
sys.path.append(gps_params['caffe_path'])
# create controller
if controller_type == 'cartpole':
C = controller.CartPole()
C.set_system_cost(x0=np.array([0, 0, 0, 0]),
u0=0.0,
Wx=np.eye(4),
Wu=1e-3)
else:
print('not implemented: ' + controller_type)
x_len = C.get_x_len()
u_len = C.get_u_len()
gps_params['x_len'] = x_len
gps_params['u_len'] = u_len
# load training data
f = h5py.File('data.h5', 'r')
xu_train_orig = f['x'].value
# if 'o' not in f:
# print( 'WARNING: observations not in data' )
# o_train_orig = None
# else:
# o_train_orig = f[ 'o' ]
# o_train_orig = o_train_orig[ ::gps_params[ 'resample' ] ]
o_train = None
if xu_train_orig.ndim == 2:
xu_train_orig = xu_train_orig[:,
1:] # TODO - at the moment 1st column is time
s = xu_train_orig.shape
xu_train_orig = xu_train_orig.reshape((1, s[0], s[1]))
resample_idx = range(0, xu_train_orig.shape[1], gps_params['resample'])
xu_train_orig = np.take(xu_train_orig, resample_idx, axis=1)
xu_train = xu_train_orig
x_train = xu_train_orig[:, :, :x_len]
u_train = xu_train_orig[:, :, x_len] # TODO check table organisation
gps = GPS(gps_params)
training_errors = []
# loop untill convergence:
for k in range(gps_params['K']):
print('running for k: {0}'.format(k))
# execute gps
policy, training_error = gps.train(xu_train, o_train,
C.get_system_cost(), gps_params)
training_errors.append(training_error)
# run on controller and collect the data
xu_run = C.run(policy)
# merge data
xu_train = gps.merge_data(xu_train, o_train, xu_run, o_run)
if b[2] & 0x08:
flags.append("(soft temp limit reached)")
return ", ".join(flags)
def startup_tasks():
"""System commands to run on service start"""
print("main: running startup tasks")
subprocess.run("/usr/bin/tvservice -o", shell=True,
capture_output=True) # disable HDMI
os.nice(-5)
gps = GPS()
compass = Compass()
threads["compass"] = threading.Thread(target=compass.track_ahrs,
args=(),
daemon=True)
threads["compass"].start()
display = Display(gps, compass)
# Blocks until the GPS is ready
display.start()
os.nice(15)
ac = Aircraft(gps)
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()
from gps import GPS
import time
from pytrack import Pytrack
from connection_otaa import Connection
from LIS2HH12 import LIS2HH12
import pycom
conn = Connection('70B3D57ED0015419', 'CA137963FA80E48A44B17973DED1977A')
py = Pytrack()
gps = GPS(py, 120)
counter = 0
while counter < 20:
if gps.hasGPS() == True:
break
else:
time.sleep(2)
gps.printGPS()
counter += 1
gps.printGPS()
print("Wakeup reason: " + str(py.get_wake_reason()) +
"; Aproximate sleep remaining: " + str(py.get_sleep_remaining()) +
" sec")
time.sleep(0.5)
# enable wakeup source from INT pin
py.setup_int_pin_wake_up(False)
import sys
from gy801 import GY801
from gps import GPS
a = GY801()
b = GPS()
print("Reading ACC data (m/s/s)")
a.accReading()
print("Reading GYRO data (degree per second )")
a.gyReading()
print("Reading MAG data")
#a.magReading()
print("Reading Temperature and Pressure")
a. airReading()
print("Reading GPS data")
b.gpsReading()
from gps import GPS from gopigo import * set_speed(75) coords = GPS(True, True) #print coords.rotation coords.turn_to_angle(180) time.sleep(10) coords.turn_to_angle(90) time.sleep(10) coords.turn_to_angle(270) time.sleep(10) coords.turn_to_angle(0) time.sleep(10) coords.stop()
import time
import cv2
import logging
import sys
from geopy import distance
import threading
from imProc import imProcessing
from capturing import ImageCap
from cmds import calculateReading, issueCommands
from globals import pixelStrip, startFrom, logInitial
from gps import GPS
if __name__ == "__main__":
gps = GPS(logging=False)
while gps.getCoords() == (0, 0):
print("Awaiting gps lock.")
#time.sleep(0.2)
gps.getGPS(force=1)
startingPos = gps.getCoords() #so we know to stop 15m afterwards
issueCommands(0, 0) #connect to car
distanceAway = 0
while distanceAway < 12: #go forward 15m
distanceAway = distance.distance(gps.getGPS(force=1), startingPos).m
print("Distance away from destination: ", distanceAway)
issueCommands(1, 100) #go straight
def main(visual, green, record, replay, loop, rc, cFlip):
calculateReading.pastCom = 0 # in case we don't see cones straight away
ic = ImageCap(False, replay) # ImCapt() #initializes zed object
startTime = time.time()
listReadings = []
gps = GPS()
while gps.getGPS() == (0, 0):
print("Waiting on GPS lock.")
time.sleep(1)
issueCommands(0, 0) #makes connection
time.sleep(3) #waits 3 seconds
issueCommands(-24, 0) #sweeps left
time.sleep(2)
issueCommands(24, 0) #sweep right
time.sleep(2)
issueCommands(0, 0)
startingPos = gps.getGPS()
running = True
timeCheck = False
ebs = False
try:
i = 0
while loop: # for amount in range(framesToDo):
logging.warning("\n*********\nFrame: " + str(i))
image, depth = ic.latest(record)
logging.info("Getting latest image and depth.")
t = threading.Thread(target=ImageCap.capture, args=(ic, ))
if not record:
original_image = image
depth_data_ocv = depth[startFrom:startFrom + pixelStrip]
image_ocv = original_image[startFrom:startFrom + pixelStrip]
steering, velocity = imProcessing(t,
image_ocv,
depth_data_ocv,
visual,
original_image,
green,
cFlip,
gates=0)
else: #looks for one gate
steering, velocity = 0, 0
cv2.imshow("image", image)
cv2.waitKey(1)
# works faster here for performance reasons
if ic.exit: # when we replay tests
raise KeyboardInterrupt
steering = min(19, max(-19, steering))
if running:
velocity = 250 #15kph
else:
velocity = 0
if distance.distance(gps.getGPS(), startingPos).m > 10:
running = False
timeCheck = time.time()
issueCommands(0, 0)
time.sleep(5)
issueCommands(0, 250)
time.sleep(2)
issueCommands.car.deploy_ebs()
raise KeyboardInterrupt
if timeCheck:
if time.time() - timeCheck > 5: #after 5 seconds
while gps.getGPS(True) == startingPos:
pass #resets starting position so we go for 10m
startingPos = gps.getGPS(True)
running = True #resets velocity to 15kph
ebs = True #it will deploy ebs once it reaches 10m again
print("Steering: ", steering)
print("Velocity: ", velocity)
logging.info("Steering: %d, Velocity: %d", steering, velocity)
issueCommands(steering, velocity, False, visual, replay, record,
rc)
listReadings.append(steering)
if not isinstance(loop, bool):
loop -= 1
if loop == 0:
raise KeyboardInterrupt
if issueCommands.car.checkEBS():
raise KeyboardInterrupt
i += 1
t.join()
i += 1
logging.warning("End of frame.\n\n")
#print("Frames left: ", framesToDo-amount)
except KeyboardInterrupt:
if not replay:
ic.zed.close()
if not ebs:
issueCommands(0, 0, False, visual, replay, record, rc)
time.sleep(4)
issueCommands(0, 0, True, visual, replay, record,
rc) #initiates the exit protocol
print("Seconds it took: ", time.time() - startTime)
print("Total frames: ", len(listReadings))
print("Actual framerate: ",
len(listReadings) / (time.time() - startTime))
print("\nFINISH")
logging.info("\nTotal seconds: %d", time.time() - startTime)
logging.info("Framerate: %d",
len(listReadings) / (time.time() - startTime))
logging.warning("Mission end.\n\n")
quit()
if d1g >= 5:
if gi0 is not None:
angle = gps_guidance(gps, motor, xbee, gi0, gi1, giG)
xbee.justSend('Go straight 10s')
print('Go straight 10s')
motor.move('straight', 10)
else:
x, ratio = camera_guidance(motor, xbee, count)
if ratio >= 0.4:
break
xbee.sendAndSave(count, gi1['time'], gi1['lat'], gi1['lon'], d1g,
angle, x, ratio)
gi0 = gi1
count += 1
xbee.sendAndSave(count, gi1['time'], gi1['lat'], gi1['lon'], d1g, angle, x,
ratio)
if __name__ == '__main__':
motor = Motor(left=(16, 12), right=(21, 20), servo_pin=23)
gps = GPS(time_zone=9, O_format='dd')
xbee = XBee()
giG = {'lat': 40.1427635, 'lon': 139.987441}
main(motor, gps, xbee, giG)
xbee.justSend('Now Shutdown')
os.system('sudo shutdown -h now')
# Main program executed by micropython
from machine import UART, RTC
from esp32 import raw_temperature
import os
import uasyncio as asyncio
from sds import SDS
from collections import namedtuple
Time = namedtuple("Time", ["Hour", "Minute", "second"])
uart_gps = UART(2, 9600)
uart_sds = UART(1, 9600)
sreader_gps = asyncio.StreamReader(uart_gps) # Create a StreamReader
from gps import GPS
# gps = AS_GPS(sreader_gps, local_offset=1) # Instantiate GPS
gps = GPS(sreader_gps, local_offset=1)
rtc = RTC()
def log(what):
print("{4:02d}:{5:02d}:{6:02d} ".format(*rtc.datetime()) + what)
def get_temprature(what=None):
if what == "header":
return " Temp."
elif what == "unit":
return "degreeC"
elif what == "text":
C = (raw_temperature() - 32) / 1.8
return "{:5.2f}".format(C)
else:
def testNoPhoneBook(self):
gps = GPS(state=['son-at-home', 'car-needs-battery', 'have-money'],
goals=['son-at-school'],
ops=self.school_ops)
self.assertEqual(gps.solve(), "can't solve")
def attach_sensor(robot, sensor_name, metalog):
assert sensor_name in ['gps', 'laser', 'camera'], sensor_name
if sensor_name == 'gps':
# 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)
robot.gps.start() # ASAP so we get GPS fix
robot.threads.append(robot.gps)
elif sensor_name == 'laser':
# Laser
laser_log_name = metalog.getLog('laser')
remission_log_name = metalog.getLog('remission')
print laser_log_name, remission_log_name
if metalog.replay:
robot.laser = DummySensor()
function = SourceLogger(None, laser_log_name).get
if remission_log_name is not None:
# remission is only optional, missing in some old log files
function2 = SourceLogger(None, remission_log_name).get
else:
robot.laser = LaserIP(remission=True)
function = SourceLogger(robot.laser.scan, laser_log_name).get
function2 = SourceLogger(robot.laser.remission,
remission_log_name).get
robot.laser_data = None
robot.register_data_source('laser', function, laser_data_extension)
robot.remission_data = None
if remission_log_name is not None:
robot.register_data_source('remission', function2,
remission_data_extension)
robot.laser.start()
robot.threads.append(robot.laser)
elif sensor_name == 'camera':
# Camera
camera_log_name = metalog.getLog('camera')
print camera_log_name
if metalog.replay:
robot.camera = DummySensor()
function = SourceLogger(None, camera_log_name).get
else:
robot.camera = Camera(sleep=0.2) # TODO
function = SourceLogger(robot.camera.lastResult,
camera_log_name).get
robot.camera_data = None
robot.register_data_source('camera', function, camera_data_extension)
robot.camera.start()
robot.threads.append(robot.camera)
else:
assert False, sensor_name # unsuported sensor
def testCarWorks(self):
gps = GPS(state=['son-at-home', 'car-works'],
goals=['son-at-school'],
ops=self.school_ops)
self.assertEqual(gps.solve(), 'solved')
import grovepi as gpi
import time
from grove_i2c_barometic_sensor_BMP180 import BMP085
from gps import GPS
from lcd import LCD
from modem import SatModem
import csv
TEMP_SENSOR = 0 # Port A0 is for the Temperature Sensor
SOUND_SENSOR = 2 # Port A2 is for the Sound Sensor
bmpSensor = BMP085(0x77, 1) # Register pressure sensor Port I2C-1
gpsSensor = GPS('/dev/ttyAMA0', 4800) # Connect Serial to GPS Device
lcdDisplay = LCD() # Create a reference to the LCD Display
satModem = SatModem('/dev/ttyUSB0') # Create a Reference to the Sat Modem
secondElapsed = 0
lcdDisplay.setText("Hello!")
time.sleep(2)
lcdDisplay.setText("Prepping Sensors!")
time.sleep(2)
lcdDisplay.setText("Ready to Go!")
time.sleep(2)
with open(str(int(time.time())) + '.csv', 'wb') as csvfile:
datawriter = csv.writer(csvfile,
delimiter=',',
quotechar='|',
quoting=csv.QUOTE_MINIMAL)
datawriter.writerow([
'temperature', 'sound', 'pressure', 'latitude', 'longitude', 'altitude'
Also send it as a response
"""
@app.route("/photo")
def photo():
print "Getting a photo"
image = camera_obj.snap()
# We'll first save the image locally
# Filename is based on position
lat = gps_obj.lat
lng = gps_obj.lng
filename = PHOTODIR + str(lat) + str(lng) + ".png"
image.save(filename) # Save the image for later
return flask.jsonify(lat=lat, lng=lng, photo=filename)
# # We'll also save the image as a buffer so we can send it by http
# fp = StringIO()
# image.save(fp, 'png')
# response = flask.make_response(fp.getvalue())
# response.headers['Content-Type'] = 'image/png'
# return response
if __name__ == "__main__":
with GPS() as gps_obj:
with Camera() as camera_obj:
app.run(debug=True)
url_for('static', filename='index.html')
from camera import Camera
from car import Car
from gps import GPS
import subprocess
import datetime
import time
import sys
import os
frameDimensions = (640, 480)
frameRate = 16
frameQuality = 30
carEnabled = False
#Initialize all the sensors
gps = GPS('COM7', 4800).start()
time.sleep(1)
if carEnabled:
car = Car('/dev/ttyAMA0', 115200, debug=True)
car.start()
time.sleep(1)
frontCamera = Camera(0,
frameDimensions,
frameRate,
label='Frontal',
verticalFlip=False).start()
time.sleep(1)
rearCamera = Camera(1,
frameDimensions,
frameRate,
label='Trasera',
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()
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)
# 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()
from gps import GPS
from time import sleep
from bsmLib import tcpClient
from time import sleep
#r = '$GPRMC,154633.000,A,4457.4413,N,09320.5487,W,0.19,0.07,011118,,,A*7B'
tcpClient(host = '192.168.21.153', port = '10002')
t = tcpClient()
t.connect()
while True:
sleep (2)
g = GPS()
r = g.read()
start = r.find('$GPRMC')
end = r.find(',0.', start)
log = r[start:end]
start2 = log.find('A')
end2 = log.find('W', start2)
log2 = log[start2:end2]
start3 = log2.find('44')
end3 = log2.find(',N', start3)
lat = log2[start3:end3]
start4 = log2.find('09')
end4 = log2.find(',W,', start4)
lon = log2[start4:end4]
