happyz
/
nexus_robot_code
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Update nexus.py

This commit is contained in:
Jacob Burroughs 2018-02-16 15:15:34 -06:00
parent 0c2387c355
commit 1b831c13da
1 changed files with 136 additions and 71 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

207
nexus.py
View File

@ -1,7 +1,11 @@
MAP_SIZE_PIXELS = 500
MAP_SIZE_METERS = 10
LIDAR_DEVICE = '/dev/ttyACM0'
ROBOT_DEVICE = '/dev/ttyUSB0'
import serial import serial
import time import time
class NexusRobot: class NexusRobot:
''' '''
Nexus Robot Library for moving Nexus Robot Library for moving
@ -27,33 +31,40 @@ class NexusRobot:
self.ser.readline() self.ser.readline()
self.ser.readline() self.ser.readline()
def get_telem(self):
self.ser.write("t;".encode())
vars = self.ser.readline().splitlines()[0].replace(";","").split(",")
if vars[0] != "telem":
return
return vars[1:]
def pause(self, duration=1): def pause(self, duration=1):
time.sleep(duration) time.sleep(duration)
def forward(self, speed, dist): def forward(self, speed, dist):
self.ser.write( self.ser.write(
"g{0},{1};".format(self._calib_spd(speed), self._calib_dis(dist)) "g{0},{1};".format(self._calib_spd(speed), self._calib_dis(dist)).encode()
) )
if dist > 0: if dist > 0:
self._wait() self._wait()
def backward(self, speed, dist): def backward(self, speed, dist):
self.ser.write( self.ser.write(
"b{0},{1};".format(self._calib_spd(speed), self._calib_dis(dist)) "b{0},{1};".format(self._calib_spd(speed), self._calib_dis(dist)).encode()
) )
if dist > 0: if dist > 0:
self._wait() self._wait()
def turnRight(self, speed, angle=-1): def turnRight(self, speed, angle=-1):
self.ser.write( self.ser.write(
"rr{0},{1};".format(self._calib_spd(speed), self._calib_ang(angle)) "rr{0},{1};".format(self._calib_spd(speed), self._calib_ang(angle)).encode()
) )
if angle > 0: if angle > 0:
self._wait() self._wait()
def turnLeft(self, speed, angle=-1): def turnLeft(self, speed, angle=-1):
self.ser.write( self.ser.write(
"rl{0},{1};".format(self._calib_spd(speed), self._calib_ang(angle)) "rl{0},{1};".format(self._calib_spd(speed), self._calib_ang(angle)).encode()
) )
if angle > 0: if angle > 0:
self._wait() self._wait()
@ -61,100 +72,154 @@ class NexusRobot:
def rotateLeft(self, speed, angle=-1): def rotateLeft(self, speed, angle=-1):
self.ser.write( self.ser.write(
"r-{0},{1},-1;".format( "r-{0},{1},-1;".format(
self._calib_spd(speed), self._calib_spd(speed)) self._calib_spd(speed), self._calib_spd(speed)).encode()
) )
# self._wait() # self._wait()
def rotateRight(self, speed, angle=-1): def rotateRight(self, speed, angle=-1):
self.ser.write( self.ser.write(
"r{0},-{1},-1;".format( "r{0},-{1},-1;".format(
self._calib_spd(speed), self._calib_spd(speed)) self._calib_spd(speed), self._calib_spd(speed)).encode()
) )
# self._wait() # self._wait()
def stop(self, speed=0): def stop(self, speed=0):
self.ser.write("s{0};".format(self._calib_spd(speed))) self.ser.write("s{0};".format(self._calib_spd(speed)).encode())
def getInfo(self): def getInfo(self):
self.ser.write("h;") self.ser.write("h;".encode())
actual_l_speed = int(self.ser.readline().rstrip().split(':')[-1]) actual_l_speed = int(self.ser.readline().rstrip().split(':')[-1])
target_l_speed = int(self.ser.readline().rstrip().split(':')[-1]) target_l_speed = int(self.ser.readline().rstrip().split(':')[-1])
actual_r_speed = int(self.ser.readline().rstrip().split(':')[-1]) actual_r_speed = int(self.ser.readline().rstrip().split(':')[-1])
target_r_speed = int(self.ser.readline().rstrip().split(':')[-1]) target_r_speed = int(self.ser.readline().rstrip().split(':')[-1])
return actual_l_speed, target_l_speed, actual_r_speed, target_r_speed return actual_l_speed, target_l_speed, actual_r_speed, target_r_speed
from breezyslam.algorithms import RMHC_SLAM
from breezyslam.sensors import XVLidar as LaserModel
from breezyslam.vehicles import WheeledVehicle
from xvlidar import XVLidar as Lidar
from pltslamshow import SlamShow
class NexusRobotOd(WheeledVehicle):
def __init__(self):
WheeledVehicle.__init__(self, 55, 1200)
def extractOdometry(self, timestamp, leftWheel, rightWheel):
return (int(timestamp) / 1000.0, int(leftWheel) / 2.08, -int(rightWheel) / 2.08)
import signal
import sys
import thread
import readchar
def telem(params):
speed = 150
prev_k = None
while 1:
k = readchar.readchar()
if k == 'w' and prev_k != k:
print('Going Forward')
robot.backward(speed, -1)
elif k == 's' and prev_k != k:
print('Going Backward')
robot.forward(speed, -1)
elif k == 'a' and prev_k != k:
print('Turing left')
robot.rotateLeft(speed, -1)
elif k == 'd' and prev_k != k:
print('Turning right')
robot.rotateRight(speed, -1)
elif k == ' ':
print('Stopping')
robot.stop()
elif k == 'h':
print('Printing info..')
print(robot.getInfo())
elif k == '+':
speed += 20
if speed > 400:
speed = 400
print('Config speed to: {0}'.format(speed))
elif k == '-':
speed -= 20
if speed < 20:
speed = 20
print('Config speed to: {0}'.format(speed))
elif k == '\x03':
robot.stop()
sys.exit(0)
else:
print('unrecognized key: {0}'.format(k))
prev_k = k
# robot.pause()
if __name__ == '__main__': if __name__ == '__main__':
import sys import sys
import tty import tty
import termios import termios
import signal
class _Getch: from threading import Thread
def __call__(self):
fd = sys.stdin.fileno()
old_settings = termios.tcgetattr(fd)
try:
tty.setraw(sys.stdin.fileno())
ch = sys.stdin.read(1)
finally:
termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
return ch
def get():
inkey = _Getch()
while 1:
k = inkey()
if k != '':
break
return k
robot = None robot = None
speed = 200 def signal_handler(signa, frame):
robot.stop()
sys.exit(0)
signal.signal(signal.SIGINT, signal_handler)
try: try:
print('Initializing..') print('Initializing..')
robot = NexusRobot('/dev/cu.usbserial-AL00YYCA') robot = NexusRobot(ROBOT_DEVICE)
print('Initialized') print('Initialized')
prev_k = None t = Thread(target=telem, args=(robot,))
while 1: t.daemon = True
k = get() t.start()
if k == 'w' and prev_k != k: odomRobot = NexusRobotOd()
print('Going Forward') # Connect to Lidar unit
robot.forward(speed, -1) lidar = Lidar(LIDAR_DEVICE)
elif k == 's' and prev_k != k:
print('Going Backward') # Create an RMHC SLAM object with a laser model and optional robot model
robot.backward(speed, -1) slam = RMHC_SLAM(LaserModel(), MAP_SIZE_PIXELS, MAP_SIZE_METERS)
elif k == 'a' and prev_k != k:
print('Turing left') # Set up a SLAM display
robot.rotateLeft(speed, -1) display = SlamShow(MAP_SIZE_PIXELS, MAP_SIZE_METERS*1000/MAP_SIZE_PIXELS, 'SLAM')
elif k == 'd' and prev_k != k:
print('Turing right') # Initialize an empty trajectory
robot.rotateRight(speed, -1) trajectory = []
elif k == ' ':
print('Stopping') # Initialize empty map
robot.stop() mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
elif k == 'h':
print('Printing info..') while True:
print(robot.getInfo()) lidar_scan = lidar.getScan()
elif k == '+': telem = robot.get_telem()
speed += 20 vel = odomRobot.computeVelocities(telem[0],telem[1],telem[2])
if speed > 400:
speed = 400 # Update SLAM with current Lidar scan, using first element of (scan, quality) pairs
print('Config speed to: {0}'.format(speed)) slam.update([pair[0] for pair in lidar_scan], vel)
elif k == '-':
speed -= 20 # Get current robot position
if speed < 20: x, y, theta = slam.getpos()
speed = 20 print '(' + str(x) + ',' + str(y) + ',' + str(theta) +')'
print('Config speed to: {0}'.format(speed))
elif k == '\x03': # Get current map bytes as grayscale
raise KeyboardInterrupt slam.getmap(mapbytes)
else:
print('unrecognized key: {0}'.format(k)) display.displayMap(mapbytes)
prev_k = k
# robot.pause() display.setPose(x, y, theta)
# Exit on ESCape
key = display.refresh()
if key != None and (key&0x1A):
robot.stop()
exit(0)
except KeyboardInterrupt:
exit(0)
except serial.serialutil.SerialException, e: except serial.serialutil.SerialException, e:
print(str(e)) print(str(e))
except KeyboardInterrupt:
if robot:
robot.stop()
except Exception: except Exception:
raise raise