Update nexus.py
This commit is contained in:
Jacob Burroughs
parent
0c2387c355
commit
1b831c13da
153
nexus.py
153
nexus.py
|
|
@ -1,7 +1,11 @@
|
|||
MAP_SIZE_PIXELS = 500
|
||||
MAP_SIZE_METERS = 10
|
||||
LIDAR_DEVICE = '/dev/ttyACM0'
|
||||
ROBOT_DEVICE = '/dev/ttyUSB0'
|
||||
|
||||
import serial
|
||||
import time
|
||||
|
||||
|
||||
class NexusRobot:
|
||||
'''
|
||||
Nexus Robot Library for moving
|
||||
|
|
@ -27,33 +31,40 @@ class NexusRobot:
|
|||
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):
|
||||
time.sleep(duration)
|
||||
|
||||
def forward(self, speed, dist):
|
||||
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:
|
||||
self._wait()
|
||||
|
||||
def backward(self, speed, dist):
|
||||
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:
|
||||
self._wait()
|
||||
|
||||
def turnRight(self, speed, angle=-1):
|
||||
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:
|
||||
self._wait()
|
||||
|
||||
def turnLeft(self, speed, angle=-1):
|
||||
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:
|
||||
self._wait()
|
||||
|
|
@ -61,73 +72,64 @@ class NexusRobot:
|
|||
def rotateLeft(self, speed, angle=-1):
|
||||
self.ser.write(
|
||||
"r-{0},{1},-1;".format(
|
||||
self._calib_spd(speed), self._calib_spd(speed))
|
||||
self._calib_spd(speed), self._calib_spd(speed)).encode()
|
||||
)
|
||||
# self._wait()
|
||||
|
||||
def rotateRight(self, speed, angle=-1):
|
||||
self.ser.write(
|
||||
"r{0},-{1},-1;".format(
|
||||
self._calib_spd(speed), self._calib_spd(speed))
|
||||
self._calib_spd(speed), self._calib_spd(speed)).encode()
|
||||
)
|
||||
# self._wait()
|
||||
|
||||
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):
|
||||
self.ser.write("h;")
|
||||
self.ser.write("h;".encode())
|
||||
actual_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])
|
||||
target_r_speed = int(self.ser.readline().rstrip().split(':')[-1])
|
||||
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
|
||||
|
||||
if __name__ == '__main__':
|
||||
import sys
|
||||
import tty
|
||||
import termios
|
||||
from xvlidar import XVLidar as Lidar
|
||||
|
||||
class _Getch:
|
||||
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
|
||||
from pltslamshow import SlamShow
|
||||
|
||||
def get():
|
||||
inkey = _Getch()
|
||||
while 1:
|
||||
k = inkey()
|
||||
if k != '':
|
||||
break
|
||||
return k
|
||||
class NexusRobotOd(WheeledVehicle):
|
||||
def __init__(self):
|
||||
WheeledVehicle.__init__(self, 55, 1200)
|
||||
|
||||
robot = None
|
||||
speed = 200
|
||||
try:
|
||||
print('Initializing..')
|
||||
robot = NexusRobot('/dev/cu.usbserial-AL00YYCA')
|
||||
print('Initialized')
|
||||
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 = get()
|
||||
k = readchar.readchar()
|
||||
if k == 'w' and prev_k != k:
|
||||
print('Going Forward')
|
||||
robot.forward(speed, -1)
|
||||
robot.backward(speed, -1)
|
||||
elif k == 's' and prev_k != k:
|
||||
print('Going Backward')
|
||||
robot.backward(speed, -1)
|
||||
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('Turing right')
|
||||
print('Turning right')
|
||||
robot.rotateRight(speed, -1)
|
||||
elif k == ' ':
|
||||
print('Stopping')
|
||||
|
|
@ -146,15 +148,78 @@ if __name__ == '__main__':
|
|||
speed = 20
|
||||
print('Config speed to: {0}'.format(speed))
|
||||
elif k == '\x03':
|
||||
raise KeyboardInterrupt
|
||||
robot.stop()
|
||||
sys.exit(0)
|
||||
else:
|
||||
print('unrecognized key: {0}'.format(k))
|
||||
prev_k = k
|
||||
# robot.pause()
|
||||
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
import sys
|
||||
import tty
|
||||
import termios
|
||||
import signal
|
||||
from threading import Thread
|
||||
|
||||
robot = None
|
||||
def signal_handler(signa, frame):
|
||||
robot.stop()
|
||||
sys.exit(0)
|
||||
signal.signal(signal.SIGINT, signal_handler)
|
||||
|
||||
try:
|
||||
print('Initializing..')
|
||||
robot = NexusRobot(ROBOT_DEVICE)
|
||||
print('Initialized')
|
||||
t = Thread(target=telem, args=(robot,))
|
||||
t.daemon = True
|
||||
t.start()
|
||||
odomRobot = NexusRobotOd()
|
||||
# Connect to Lidar unit
|
||||
lidar = Lidar(LIDAR_DEVICE)
|
||||
|
||||
# Create an RMHC SLAM object with a laser model and optional robot model
|
||||
slam = RMHC_SLAM(LaserModel(), MAP_SIZE_PIXELS, MAP_SIZE_METERS)
|
||||
|
||||
# Set up a SLAM display
|
||||
display = SlamShow(MAP_SIZE_PIXELS, MAP_SIZE_METERS*1000/MAP_SIZE_PIXELS, 'SLAM')
|
||||
|
||||
# Initialize an empty trajectory
|
||||
trajectory = []
|
||||
|
||||
# Initialize empty map
|
||||
mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
|
||||
|
||||
while True:
|
||||
lidar_scan = lidar.getScan()
|
||||
telem = robot.get_telem()
|
||||
vel = odomRobot.computeVelocities(telem[0],telem[1],telem[2])
|
||||
|
||||
# Update SLAM with current Lidar scan, using first element of (scan, quality) pairs
|
||||
slam.update([pair[0] for pair in lidar_scan], vel)
|
||||
|
||||
# Get current robot position
|
||||
x, y, theta = slam.getpos()
|
||||
print '(' + str(x) + ',' + str(y) + ',' + str(theta) +')'
|
||||
|
||||
# Get current map bytes as grayscale
|
||||
slam.getmap(mapbytes)
|
||||
|
||||
display.displayMap(mapbytes)
|
||||
|
||||
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:
|
||||
print(str(e))
|
||||
except KeyboardInterrupt:
|
||||
if robot:
|
||||
robot.stop()
|
||||
except Exception:
|
||||
raise
|
||||
|
|
|
|||
Loading…
Reference in New Issue