class SLAM:
"""
takes a breezyslam laser object and a flag to determine the
slam algorithms that is used.
"""
def __init__(self, lidar_turret):
self.map_size_pixels = 1600
self.map_size_meters = 50
self.trajectory = []
self.mapbytes = bytearray(self.map_size_pixels * self.map_size_pixels)
self.laser = Laser(lidar_turret.point_cloud_size, 2.4,
lidar_turret.detection_angle_degrees, 0)
self.algorithm = RMHC_SLAM(self.laser, self.map_size_pixels,
self.map_size_meters)
def update(self, timestamp, point_cloud, velocity):
self.algorithm.update(point_cloud, velocity)
x_mm, y_mm, theta_degrees = self.algorithm.getpos()
self.trajectory.append((x_mm, y_mm))
self.algorithm.getmap(self.mapbytes)
def make_image(self, image_name, image_format="pgm"):
self.algorithm.getmap(self.mapbytes)
for coords in self.trajectory:
x_mm, y_mm = coords
x_pix = self.mm2pix(x_mm)
y_pix = self.mm2pix(y_mm)
self.mapbytes[y_pix * self.map_size_pixels + x_pix] = 0
if image_format == "pgm":
pgm_save(image_name + "." + image_format, self.mapbytes,
(self.map_size_pixels, self.map_size_pixels))
else:
image = Image.frombuffer(
'L', (self.map_size_pixels, self.map_size_pixels),
self.mapbytes, 'raw', 'L', 0, 1)
image.save(image_name + "." + image_format)
def get_pos(self):
return self.algorithm.getpos()
def mm2pix(self, mm):
return int(mm / (self.map_size_meters * 1000 / self.map_size_pixels))
def main():
# 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
viz = MapVisualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS, 'SLAM')
# Initialize an empty trajectory
trajectory = []
# Initialize empty map
mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
# Create an iterator to collect scan data from the RPLidar
iterator = lidar.iter_scans()
# We will use these to store previous scan in case current scan is inadequate
previous_distances = None
previous_angles = None
# First scan is crap, so ignore it
next(iterator)
while True:
# Extract (quality, angle, distance) triples from current scan
items = [item for item in next(iterator)]
# Extract distances and angles from triples
distances = [item[2] for item in items]
print(distances)
angles = [item[1] for item in items]
# Update SLAM with current Lidar scan and scan angles if adequate
if len(distances) > MIN_SAMPLES:
slam.update(distances, scan_angles_degrees=angles)
previous_distances = distances.copy()
previous_angles = angles.copy()
# If not adequate, use previous
elif previous_distances is not None:
slam.update(previous_distances,
scan_angles_degrees=previous_angles)
# Get current robot position
x, y, theta = slam.getpos()
# Get current map bytes as grayscale
slam.getmap(mapbytes)
# Display map and robot pose, exiting gracefully if user closes it
if not viz.display(x / 1000., y / 1000., theta, mapbytes):
exit(0)
# Shut down the lidar connection
lidar.stop()
lidar.disconnect()
def main():
print "initiating engine..."
seed = 9999
dequeueCount = 0
time_start = time.time()
targetProcess = "node server.js"
targetSignal = signal.SIGUSR1
serverMinTimeDelay = 5
pid = getpid(targetProcess)
slam = RMHC_SLAM(MinesLaser(), MAP_SIZE_PIXELS, MAP_SIZE_METERS, random_seed=seed) \
if seed \
else Deterministic_SLAM(MinesLaser(), MAP_SIZE_PIXELS, MAP_SIZE_METERS)
trajectory = []
while time.time() - time_start < 600:
time_thisLoop = time.time()
if not q.empty():
while (not q.empty()):
slam.update(q.get())
dequeueCount = dequeueCount + 1
x_mm, y_mm, theta_degrees = slam.getpos()
trajectory.append((x_mm, y_mm))
# Create a byte array to receive the computed maps
mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
# Get map from this batch
slam.getmap(mapbytes)
# Put trajectory into map as black pixels
for coords in trajectory:
x_mm, y_mm = coords
x_pix = mm2pix(x_mm)
y_pix = mm2pix(y_mm)
mapbytes[y_pix * MAP_SIZE_PIXELS + x_pix] = 0;
# Save map and trajectory as PNG file
image = Image.frombuffer('L', (MAP_SIZE_PIXELS, MAP_SIZE_PIXELS), mapbytes, 'raw', 'L', 0, 1)
image.save("../../webgui/images/" + "map" + ".png")
print "map created after {} scans".format(dequeueCount)
# Try to tell the server
if pid == False:
print "failure: no server pid"
pid = getpid(targetProcess)
else:
try:
os.kill(int(pid),targetSignal)
print "success: signal sent to server"
except OSError:
print "error: whoops, just lost the pid"
pid = getpid(targetProcess)
# give the server at least serverMinTimeDelay seconds to catch up
while time.time() - time_thisLoop < serverMinTimeDelay:
()
class BreezySLAM(object):
'''
https://github.com/simondlevy/BreezySLAM
'''
def __init__(self, MAP_SIZE_PIXELS=500, MAP_SIZE_METERS=10):
from breezyslam.algorithms import RMHC_SLAM
from breezyslam.sensors import Laser
laser_model = Laser(scan_size=360,
scan_rate_hz=10.,
detection_angle_degrees=360,
distance_no_detection_mm=12000)
MAP_QUALITY = 5
self.slam = RMHC_SLAM(laser_model, MAP_SIZE_PIXELS, MAP_SIZE_METERS,
MAP_QUALITY)
def run(self, distances, angles, map_bytes):
self.slam.update(distances, scan_angles_degrees=angles)
x, y, theta = self.slam.getpos()
if map_bytes is not None:
self.slam.getmap(map_bytes)
#print('x', x, 'y', y, 'theta', norm_deg(theta))
return x, y, deg2rad(norm_deg(theta))
def shutdown(self):
pass
class Lidar(BaseLidar):
def __init__(self, on_map_change):
super().__init__(on_map_change=on_map_change)
self.lidar = RPLidar(os.getenv('LIDAR_DEVICE', SLAM["LIDAR_DEVICE"]))
self.slam = RMHC_SLAM(RPLidarA1(), SLAM['MAP_SIZE_PIXELS'], SLAM['MAP_SIZE_METERS'])
self.map_size_pixels = SLAM['MAP_SIZE_PIXELS']
self.trajectory = []
self.mapbytes = bytearray(SLAM['MAP_SIZE_PIXELS'] * SLAM['MAP_SIZE_PIXELS'])
self.prev_checksum = 0
def stop(self):
# await super().stop()
self.lidar.stop()
self.lidar.stop_motor()
self.lidar.disconnect()
def run(self):
try:
previous_distances = None
previous_angles = None
iterator = self.lidar.iter_scans()
next(iterator)
while True:
items = [(q, 360 - angle, dist) for q, angle, dist in next(iterator)]
distances = [item[2] for item in items]
angles = [item[1] for item in items]
if len(distances) > SLAM['MIN_SAMPLES']:
self.slam.update(distances, scan_angles_degrees=angles)
previous_distances = distances.copy()
previous_angles = angles.copy()
elif previous_distances is not None:
self.slam.update(previous_distances, scan_angles_degrees=previous_angles)
x, y, theta = self.slam.getpos()
self.trajectory.append((x, y))
self.slam.getmap(self.mapbytes)
for coords in self.trajectory:
x_mm, y_mm = coords
x_pix = mm2pix(x_mm)
y_pix = mm2pix(y_mm)
self.mapbytes[y_pix * SLAM['MAP_SIZE_PIXELS'] + x_pix] = 0
checksum = sum(self.mapbytes)
if self.on_map_change and checksum != self.prev_checksum:
# print(checksum)
x = Image.frombuffer('L', (self.map_size_pixels, self.map_size_pixels), self.mapbytes, 'raw', 'L', 0, 1)
bytes_img = io.BytesIO()
x.save(bytes_img, format='PNG')
self.on_map_change(bytearray(bytes_img.getvalue()))
self.prev_checksum = checksum
except Exception as e:
print(e)
finally:
self.stop()
def __init__(self):
from breezyslam.algorithms import RMHC_SLAM
lidar = MyLidarModel()
mapbytes = bytearray(800 * 800)
slam = RMHC_SLAM(lidar, 800, 35)
while True:
scan = readLidar()
slam.update(scan)
x, y, theta = slam.getpos(scan)
slam.getmap(mapbytes)
def main():
lidar = Lidar(device='/dev/ttyACM0')
slam = RMHC_SLAM(LaserModel(), map_size_pixels=MAP_SIZE_PIXELS, map_size_meters=MAP_SIZE_METERS)
display = SlamShow(MAP_SIZE_PIXELS, MAP_SIZE_METERS*1000/MAP_SIZE_PIXELS, 'SLAM')
#image_thread = threading.Thread(target=save_image, args=[display])
#image_thread.start()
mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
while True:
slam.update(lidar.getScan())
x, y, theta = slam.getpos()
#print(x, y, theta)
slam.getmap(mapbytes)
display.displayMap(mapbytes)
display.setPose(x, y, theta)
#display.save_image()
display.save_pgm(mapbytes)
if not display.refresh():
exit(0)
def main():
seed = 9999
runCount = 0
dequeueCount = 0
slam = RMHC_SLAM(MinesLaser(), MAP_SIZE_PIXELS, MAP_SIZE_METERS, random_seed=seed) \
if seed \
else Deterministic_SLAM(MinesLaser(), MAP_SIZE_PIXELS, MAP_SIZE_METERS)
trajectory = []
while dequeueCount < 1000:
time.sleep(10)
if q.empty() == False:
while (q.empty() == False):
slam.update(q.get())
#print "%i" %dequeueCount
dequeueCount = dequeueCount + 1
x_mm, y_mm, theta_degrees = slam.getpos()
trajectory.append((x_mm, y_mm))
# Create a byte array to receive the computed maps
mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
# Get final map
slam.getmap(mapbytes)
# Put trajectory into map as black pixels
for coords in trajectory:
x_mm, y_mm = coords
x_pix = mm2pix(x_mm)
y_pix = mm2pix(y_mm)
mapbytes[y_pix * MAP_SIZE_PIXELS + x_pix] = 0
# Save map and trajectory as PNG file
image = Image.frombuffer('L', (MAP_SIZE_PIXELS, MAP_SIZE_PIXELS),
mapbytes, 'raw', 'L', 0, 1)
#image.save('map%i.png' %runCount)
#image.save("/home/card/webgui/images/" + "map" + str(dequeueCount) + ".png")
image.save("/home/card/webgui/images/" + "map" + ".png")
def start_simulation(self):
self.set_motor_speed(0.8, 0)
(errorCode, detectionState, detectedPoint, detectedObjectHandle,
detectedSurfaceNormalVector) = vrep.simxReadProximitySensor(
self.client_id, self.proximity_handle, vrep.simx_opmode_streaming)
e, data = vrep.simxGetStringSignal(self.client_id, "lidarMeasuredData",
vrep.simx_opmode_streaming)
self.check_error_code(e, "simxGetStringSignal lidar error")
# 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
viz = MapVisualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS, 'SLAM')
# Initialize empty map
mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
res, v0 = vrep.simxGetObjectHandle(self.client_id, 'Vision_sensor',
vrep.simx_opmode_oneshot_wait)
res, resolution, image = vrep.simxGetVisionSensorImage(
self.client_id, v0, 0, vrep.simx_opmode_streaming)
while vrep.simxGetConnectionId(self.client_id) != -1:
is_detected, distance, vector = self.get_proximity_data()
if is_detected:
print("distance: {} {}".format(distance, vector))
e, lidar_data, dist_data = self.get_lidar_data()
point_data_len = len(lidar_data)
dist_data_len = len(dist_data)
dist_data = dist_data[::-1]
print("points len: {}; dist len: {}".format(
point_data_len, dist_data_len))
self.draw_lidar_data(dist_data[0:-2])
# Update SLAM , with current Lidar scan
slam.update(dist_data[0:-2])
# Get current robot position
x, y, theta = slam.getpos()
print("x: {}; y: {}; theta: {}".format(x, y, theta))
# Get current map bytes as grayscale
slam.getmap(mapbytes)
# Display map and robot pose, exiting gracefully if user closes it
if not viz.display(x / 1000., y / 1000., theta, mapbytes):
exit(0)
err, resolution, image = vrep.simxGetVisionSensorImage(
self.client_id, v0, 0, vrep.simx_opmode_buffer)
if err == vrep.simx_return_ok:
img = np.array(image, dtype=np.uint8)
img.resize([resolution[1], resolution[0], 3])
fimg = cv2.flip(img, 0)
cv2.imshow('vision sensor', fimg)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
elif err == vrep.simx_return_novalue_flag:
print("no image yet")
pass
else:
print(err)
simulationTime = vrep.simxGetLastCmdTime(self.client_id)
time.sleep(0.1)
vrep.simxFinish(self.client_id)
class Map(Thread):
MAP_SIZE_PIXELS = 1000
MAP_SIZE_METERS = 7
LIDAR_DEVICE = '/dev/ttyACM0'
THRESH_DETECTION = 100
RAW_ANGLE = np.pi/4
LIDAR_POS = np.array([140,140])
def __init__(self):
Thread.__init__(self)
#self.lidar = Lidar(LIDAR_DEVICE)
self.slam = RMHC_SLAM(LaserModel(), MAP_SIZE_PIXELS, MAP_SIZE_METERS, map_quality=50, hole_width_mm=200, max_search_iter=10000)
self.N = 0
self.bot_pos = [[1500, 1300]]
self.bot_ori = [0]
self.state = True
def run(self):
self.i = 0
x_th = 1500
y_th = 1300
while(self.state):
if(self.i<10):
y_th +=35
elif(self.i<33):
x_th -= 45
elif(self.i<45):
y_th -= 35
else:
y_th -= 10
x_th += 10
# Update SLAM with current Lidar scan
#self.slam.update(self.lidar.getScan())
data = getHokuyoData([x_th, y_th],
-self.i*5/180.0*np.pi, [np.array([[0, 2000, 2000, 0],[0, 0, 3000, 3000]]),
np.array([[650, 1350, 1350, 650],[1450, 1450, 1550, 1550]]),
np.array([[1500, 1600, 1600, 1500],[1000, 1000, 1100, 1100]]),
np.array([[1000+200*np.cos(k*10/180*np.pi) for k in range(0,36)],
[2500+200*np.sin(k*10/180*np.pi) for k in range(0,36)]])], 0)
list = data[1].tolist()
if(len(list) == 682):
self.slam.update(list, [0,0,0.1])
# Get current robot position
y, x, theta = self.slam.getpos()
x-=3500
y-=3500
x = -x
#y-=198
x+=1500
y+=1300
#angle = self.RAW_ANGLE + theta*3.1415/180
#R = np.array([[np.cos(angle), -np.sin(angle)],[np.sin(angle), np.cos(angle)]])
#pos = R.dot(np.array([x,y]))
pos = np.array([x,y])
self.bot_pos.append(pos)
self.bot_ori.append(theta)
self.i += 1
def stop(self):
self.state = False
class Slam(Node):
"""
SLAM NODE
This node is in charge of receiving data from the LIDAR and creating a map of the environment
while locating the robot in the map.
INPUT
- Bottle detected by the Neuron (to be removed soon)
- Lidar detections
OUTPUT
- Robot position: /robot_pos
- Map as a byte array: /world_map
(this map can be interpred using the map_utils.py package !)
"""
def __init__(self):
super().__init__('slam_node')
# Create second subscription (SLAM)
self.subscription2 = self.create_subscription(
LidarData, 'lidar_data', self.listener_callback_lidar, 100)
self.subscription2 # prevent unused variable warning
# Create third subscription (Motor speeds to compute robot pose change)
self.subscription3 = self.create_subscription(
MotorsSpeed, 'motors_speed', self.listener_callback_motorsspeed,
1000)
self.subscription3 # prevent unused variable warning
# create subscription to control map_quality
self.slam_control_sub = self.create_subscription(
String, 'slam_control', self.slam_control_callback, 1000)
# Create publisher for the position of the robot, and for the map
self.publisher_position = self.create_publisher(
Position, 'robot_pos', 10)
self.publisher_map = self.create_publisher(Map, 'world_map', 10)
# Initialize parameters for slam
laser = LaserModel(detection_margin=DETECTION_MARGIN,
offset_mm=OFFSET_MM)
self.slam = RMHC_SLAM(laser,
MAP_SIZE_PIXELS,
MAP_SIZE_METERS,
map_quality=MAP_QUALITY,
hole_width_mm=OBSTACLE_WIDTH_MM,
x0_mm=X0,
y0_mm=Y0,
theta0=0,
sigma_xy_mm=DEFAULT_SIGMA_XY_MM)
self.trajectory = []
self.mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
self.previous_distances = None
self.previous_angles = None
self.robot_pose_change = np.array([0.0, 0.0, 0.0])
self.is_map_quality_high = True
# DEBUG parameters
self.map_index = 0
self.previous_pos = (0, 0, 0)
self.s = time.time()
print("SLAM is starting")
def slam_control_callback(self, msg):
"""Called to changed the map quality
"""
if msg.data == "save_state":
print("Slam is saving current state")
self.last_valid_map = self.mapbytes.copy()
self.last_valid_pos = self.slam.getpos()
elif msg.data == "recover_state":
# METHOD 1
self.slam.setmap(self.last_valid_map)
## self.slam.setpos(self.last_valid_pos)
elif msg.data == "freeze":
self.last_valid_map = self.mapbytes.copy()
self.last_valid_pos = self.slam.getpos()
elif msg.data == "unfreeze":
self.slam.setmap(self.last_valid_map)
# self.slam.setpos(self.last_valid_pos)
return
##########################
print("changing quality")
if self.is_map_quality_high:
#self.previous_map = self.mapbytes.copy()
self.slam.map_quality = 0
self.slam.sigma_xy_mm = 5 * DEFAULT_SIGMA_XY_MM
else:
#self.slam.setmap(self.previous_map)
self.slam.map_quality = MAP_QUALITY
self.slam.sigma_xy_mm = DEFAULT_SIGMA_XY_MM
self.is_map_quality_high = not self.is_map_quality_high
def listener_callback_motorsspeed(self, msg):
"""
Must compute the robot pose change since the last time the data was collected from the motor
The input are the motors speed and the time change.
Documentation of the SLAM library says :
"pose_change is a tuple (dxy_mm, dtheta_degrees, dt_seconds) computed from odometry"
"""
dx_left = msg.RADIUS * msg.left * (0.1047) * msg.time_delta
dx_right = msg.RADIUS * msg.right * (0.1047) * msg.time_delta
delta_r = 1 / 2 * (dx_left + dx_right) # [mm]
delta_d = -(dx_right - dx_left)
delta_theta = np.arctan(delta_d / msg.LENGTH) * 57.2958 # [deg]
self.robot_pose_change += [delta_r, delta_theta, msg.time_delta]
def listener_callback_lidar(self, msg):
# transform angles to $FORMAT1
angles = np.array(msg.angles)
angles = list(
(angles + 180) % 360) # because LIDAR is facing the robot
distances = list(msg.distances)
# Update SLAM with current Lidar scan and scan angles if adequate
if len(distances) > MIN_SAMPLES:
e = time.time()
self.s = e
self.slam.update(distances,
scan_angles_degrees=angles,
pose_change=tuple(self.robot_pose_change))
# self.analyse_odometry()
self.robot_pose_change = np.array([0.0, 0.0, 0.0])
self.previous_distances = distances.copy()
self.previous_angles = angles.copy()
elif self.previous_distances is not None:
# If not adequate, use previous
print("Slam NOT updating")
self.slam.update(self.previous_distances,
scan_angles_degrees=self.previous_angles)
# Get current robot position and current map bytes as grayscale
x, y, theta = self.slam.getpos()
self.slam.getmap(self.mapbytes)
# Send topics
pos = Position()
pos.x = float(x)
pos.y = float(y)
pos.theta = float(theta)
self.publisher_position.publish(pos)
map_message = Map()
map_message.map_data = self.mapbytes
map_message.index = self.map_index
self.publisher_map.publish(map_message)
print("Map sent: ", self.map_index)
self.map_index += 1
# debug functions here (do NOT call them on real runs)
def analyse_odometry(self):
"""Compare the odometry resuls with the change in position according to SLAM.
For this function to work, self.robot_pose_change must be set to something different than zero.
And the variabe self.previous_pos must exist
"""
x, y, theta = self.slam.getpos()
x_old, y_old, theta_old = self.previous_pos
dr = np.sqrt((x - x_old)**2 + (y - y_old)**2)
dtheta = theta - theta_old
print("-------")
print("SLAM change of pos: {:.3f} {:.3f}".format(dr, dtheta))
print("Odometry change of pos: {:.3f} {:.3f} ".format(
self.robot_pose_change[0], self.robot_pose_change[1]))
self.previous_pos = (x, y, theta)
def find_lidar_range(self, distances, angles):
"""Finds the critical angles of the LIDAR and prints them.
Careful, this function works (currently) only with the previous angles format.
"""
(i1, i2) = lidar_utils.get_valid_lidar_range(distances=distances,
angles=angles,
n_points=10)
print('indexes : {} : {} with angles {} - {}'.format(
i1, i2, angles[i1], angles[i2]))
def slam(currentProgram):
trajectory = []
# Connect to Lidar unit
try:
lidar = Lidar(PORT0)
currentProgram.roombaPort = PORT1
iterator = lidar.iter_scans(1000)
lidar.stop()
next(iterator)
print("Lidar 0, Roomba 1")
except:
print("Roomba 0, Lidar 1")
lidar.stop()
lidar.disconnect()
lidar = Lidar(PORT1)
currentProgram.roombaPort = PORT0
iterator = lidar.iter_scans(1000)
lidar.stop()
next(iterator)
# Create an RMHC SLAM object with a laser model and optional robot model
slam = RMHC_SLAM(LaserModel(), MAP_SIZE_PIXELS, MAP_SIZE_METERS)
trajectory = []
previous_distances = None
previous_angles = None
# start time
start_time = time.time()
prevTime = start_time
trigger_start = -100
while (not currentProgram.stop):
SLAMvel = currentProgram.SLAMvals[0]
SLAMrot = currentProgram.SLAMvals[1]
# Extract (quality, angle, distance) triples from current scan
items = [item for item in next(iterator)]
# Extract distances and angles from triples
distances = [item[2] for item in items]
angles = [item[1] for item in items]
l = list(zip(angles,distances))
filtered = list(filter(lambda e: (e[0]>=45 and e[0]<=135) and e[1]<300 , l))
# s = sorted(l, key = lambda e: e[0])
if (len(filtered) > constants.POINTS_THRESHOLD) and (time.time()-trigger_start >5):
currentProgram.programStatus = constants.Status.LIDAR_OBSTACLE
topleft = list(filter(lambda e: (e[0]>=105 and e[0]<=135) , filtered))
front = list(filter(lambda e: (e[0]>=75 and e[0]<=105) , filtered))
topright = list(filter(lambda e: (e[0]>=45 and e[0]<=75) , filtered))
if (len(topleft) > 2):
currentProgram.obstacleLocation[0] = 1
if (len(front) > 2):
currentProgram.obstacleLocation[1] = 1
if (len(topright) > 2):
currentProgram.obstacleLocation[2] = 1
trigger_start = time.time()
# Update SLAM with current Lidar scan and scan angles if adequate
if len(distances) > MIN_SAMPLES:
# print("using speeds ", SLAMvel, SLAMrot)
dt = time.time() - prevTime
slam.update(distances, pose_change=(
(SLAMvel*dt, SLAMrot*dt, dt)), scan_angles_degrees=angles)
prevTime = time.time()
previous_distances = copy.copy(distances)
previous_angles = copy.copy(angles)
# print("updated - if")
# If not adequate, use previous
elif previous_distances is not None:
# print("using speeds ", SLAMvel, SLAMrot)
dt = time.time() - prevTime
slam.update(previous_distances, pose_change=(
(SLAMvel*dt, SLAMrot*dt, dt)), scan_angles_degrees=previous_angles)
prevTime = time.time()
# print("updated - else")
# Get current robot position
x, y, theta = slam.getpos()
[x_pix, y_pix] = [mm2pix(x), mm2pix(y)]
currentProgram.robot_pos = [
y_pix // constants.CHUNK_SIZE, x_pix // constants.CHUNK_SIZE]
# print("robot_pos - ",x_pix // constants.CHUNK_SIZE,y_pix // constants.CHUNK_SIZE, theta)
# Get current map bytes as grayscale
slam.getmap(currentProgram.mapbytes)
# Shut down the lidar connection
pgm_save('ok.pgm', currentProgram.mapbytes,
(MAP_SIZE_PIXELS, MAP_SIZE_PIXELS))
lidar.stop()
lidar.disconnect()
def main():
# Bozo filter for input args
if len(argv) < 3:
print('Usage: %s <dataset> <use_odometry> [random_seed]' % argv[0])
print('Example: %s exp2 1 9999' % argv[0])
exit(1)
# Grab input args
dataset = argv[1]
use_odometry = True if int(argv[2]) else False
seed = int(argv[3]) if len(argv) > 3 else 0
# Build a robot model if we want odometry
robot = Rover() if use_odometry else None
lidarobj = Laser(360, 12, 360, 8000)
# Create a CoreSLAM object with laser params and robot object
slam = RMHC_SLAM(lidarobj, MAP_SIZE_PIXELS, MAP_SIZE_METERS, random_seed=seed) \
if seed \
else Deterministic_SLAM(MinesLaser(), MAP_SIZE_PIXELS, MAP_SIZE_METERS)
# Start with an empty trajectory of positions
trajectory = []
mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
suffix = 1
while (True):
if (use_odometry):
mutex.acquire()
mainLCMQ = lcmQ
mainODOQ = odoQ
# Clear Queues once copied from thread into main for next batch of data
lcmQ.queue.clear()
odoQ.queue.clear()
mutex.release()
velocities = robot.computePoseChange(mainODOQ.get())
slam.update(mainLCMQ.get(), velocities)
x_mm, y_mm, theta_degrees = slam.getpos()
x_pix = mm2pix(x_mm)
y_pix = mm2pix(y_mm)
trajectory.append((y_pix, x_pix))
slam.getmap(mapbytes)
trajLen = len(trajectory)
for i in range(trajLen):
if (i == (trajLen - 1)):
mapbytes[trajectory[i][0] * MAP_SIZE_PIXELS +
trajectory[i][1]] = 0
else:
mapbytes[trajectory[i][0] * MAP_SIZE_PIXELS +
trajectory[i][1]] = 120
filename = dataset + str(suffix)
pgm_save('%s.pgm' % filename, mapbytes,
(MAP_SIZE_PIXELS, MAP_SIZE_PIXELS))
suffix += 1
if (keyPressed == 's'):
#Wrap up last map using leftover data
pgm_save('%s.pgm' % filename, mapbytes,
(MAP_SIZE_PIXELS, MAP_SIZE_PIXELS))
'''
This will take all the maps generated and place them into pgmbagfolder
For this to work, make sure your destination directory has a folder called pgmbagfolder
Change the directory:
/home/Shaurya98/rplidar_workspace/src/mapping/BreezySLAM/examples
and
/home/Shaurya98/rplidar_workspace/src/mapping/BreezySLAM/examples/pgmbagfolder
With your own destination directory. It it recommended to put pgmbagfolder under the examples
directory
'''
os.chdir(
"/home/pi/rplidar_workspace/src/mapping/BreezySLAM/examples/pgmbagfolder"
)
for pgm_file in glob.iglob('*.pgm'):
os.remove(pgm_file)
print("\nEmptied pgmbagfolder")
os.chdir(
"/home/pi/rplidar_workspace/src/mapping/BreezySLAM/examples"
)
for pgm_file in glob.iglob('*.pgm'):
shutil.copy2(
pgm_file,
"/home/pi/rplidar_workspace/src/mapping/BreezySLAM/examples/pgmbagfolder"
)
os.remove(pgm_file)
print("\nFiles recorded and sent to pgmbagfolder")
#Terminate threads before exiting main()
thread1.join()
thread2.join()
thread3.join()
break
class SLAM_Engine:
def __init__(self, size_pixels, size_meters):
self.map_size_pixels = size_pixels
self.map_size_meters = size_meters
self.map_scale_meters_per_pixel = float(size_meters) / float(
size_pixels)
self.mapbytes = bytearray(self.map_size_pixels * self.map_size_pixels)
self.robot = None
self.control = False
self.lidarShow = True
self.routine = False
#slam算法初始化
self.slam = RMHC_SLAM(MinesLaser(),
self.map_size_pixels,
self.map_size_meters,
map_quality=_DEFAULT_MAP_QUALITY,
hole_width_mm=_DEFAULT_HOLE_WIDTH_MM,
random_seed=999,
sigma_xy_mm=_DEFAULT_SIGMA_XY_MM,
sigma_theta_degrees=_DEFAULT_SIGMA_THETA_DEGREES,
max_search_iter=_DEFAULT_MAX_SEARCH_ITER)
self.map_view = OpenMap(self.map_size_pixels, self.map_size_meters)
self.pose = [0, 0, 0]
def set_auto_control(self):
self.control = True
def reset_auto_control(self):
self.control = False
self.map_view.path = []
self.map_view.point_list = []
def data_test(self, dataset='expX'):
timestamps, lidars, odometries = load_data_EAI('.', dataset)
for scanno in range(len(lidars)):
self.slam_run(lidars[scanno])
self.slam_view()
def NoLidarInit(self, dataset='expX'):
self.timestamps, self.lidars, self.odometries = load_data_EAI(
'.', dataset)
self.SLAM_continue = False
def NoLidarGetScans(self):
scans_m = []
if len(self.lidars) > 1:
scans = self.lidars.pop(0)
for scan in scans:
scans_m.append(float(scan) / 1000.0)
return scans, scans_m
else:
scans = self.lidars[0]
for scan in scans:
scans_m.append(float(scan) / 1000.0)
return scans, scans_m
def routine_switch(self):
self.map_view.path = self.map_view.routinePath.copy()
self.map_view.routinePath.reverse()
self.map_view.END_flag = False
def slam_control(self):
if self.control == True:
return self.map_view.track_control()
else:
return
def slam_run(self, scans):
self.slam.update(scans) #地图更新
self.pose[0], self.pose[1], self.pose[2] = self.slam.getpos() #位置预测
self.slam.getmap(self.mapbytes) #地图读取
self.map_view.setPose(self.pose[0] / 1000., self.pose[1] / 1000.,
self.pose[2])
def slam_view(self):
self.map_view.display(self.pose[0] / 1000.,
self.pose[1] / 1000.,
self.pose[2],
self.mapbytes,
test=False)
def slam_mjpg(self):
s = self.map_scale_meters_per_pixel
self.map_view.display_mjpg(self.pose[0] / 1000.,
self.pose[1] / 1000.,
self.pose[2],
self.mapbytes,
test=False)
pos = [
self.map_size_pixels - int((self.pose[0] / 1000.) / s),
int((self.pose[1] / 1000.) / s), self.pose[2]
]
return self.map_view.get_jpg(), pos
def setPoint(self, x, y):
return self.map_view.setPoint(self.map_size_pixels - x, y)
def _m2pix(self, x_m, y_m):
s = self.map_scale_meters_per_pixel
#p = self.map_size_pixels
return int(x_m / s), int(y_m / s)
def draw_history(self):
self.map_view.get_history_pose()
def del_history(self):
self.map_view.del_history_pose()
class Main():
def __init__(self):
self.b = True
self.MIN_SAMPLES = 20
while self.b == True:
try:
self.init_lidar()
self.b = False
except Exception as e:
print(e)
self.init_slam()
def init_lidar(self):
self.lidar = Lidar('/dev/ttyUSB0')
self.iterator = self.lidar.iter_scans(1000)
time.sleep(0.5)
# First scan is crap, so ignore it
next(self.iterator)
def init_slam(self):
self.MAP_SIZE_PIXELS = cfg.MAP_SIZE_PIXELS
self.MAP_SIZE_METERS = cfg.MAP_SIZE_METERS
self.slam = RMHC_SLAM(LaserModel(), self.MAP_SIZE_PIXELS,
self.MAP_SIZE_METERS)
self.mapbytes = bytearray(self.MAP_SIZE_PIXELS * self.MAP_SIZE_PIXELS)
def img_processing(self, mapimg):
t_map = ip.thresholding(mapimg)
d_map = ip.dilation(t_map)
e_map = ip.erosion(d_map)
blur = ip.blur(e_map, cfg.MAP_BLUR, t=0)
return blur
#cv2.imwrite("map2.jpg", blur)
def main_loop(self):
var = 0
time_total = 0
total_matches = 0
max_matches = 0
while var < 50:
# Extract (quality, angle, distance) triples from current scan
items = [item for item in next(self.iterator)]
# Extract distances and angles from triples
distances = [item[2] for item in items]
angles = [item[1] for item in items]
# Update SLAM with current Lidar scan and scan angles if adequate
if len(distances) > self.MIN_SAMPLES:
self.slam.update(distances, scan_angles_degrees=angles)
self.previous_distances = distances.copy()
self.previous_angles = angles.copy()
# If not adequate, use previous
elif self.previous_distances is not None:
self.slam.update(self.previous_distances,
scan_angles_degrees=self.previous_angles)
# Get current robot position
x, y, theta = self.slam.getpos()
# Get current map bytes as grayscale
self.slam.getmap(self.mapbytes)
mapimg = np.reshape(np.frombuffer(self.mapbytes, dtype=np.uint8),
(self.MAP_SIZE_PIXELS, self.MAP_SIZE_PIXELS))
t1 = time.time()
blur = self.img_processing(mapimg)
a, b, matches = ip.find_spot(blur)
t2 = time.time() - t1
if matches > max_matches:
max_matches = matches
time_total += t2
total_matches += matches
var += 1
avg_time = time_total / var
avg_matches = total_matches / var
print("avg_matches: " + str(avg_matches))
print("max_matches: " + str(max_matches))
print("avg_time: " + str(avg_time))
# Update SLAM with current Lidar scan and scan angles if adequate
if len(distances) > MIN_SAMPLES:
# First interpolate to get 360 angles from 0 through 359, and corresponding distances
f = interp1d(angles, distances, fill_value='extrapolate')
distances = list(f(np.arange(360))) # slam.update wants a list
# Then update with interpolated distances
slam.update(distances)
# Store interplated distances for next time
previous_distances = distances.copy()
# If not adequate, use previous
elif previous_distances is not None:
slam.update(previous_distances)
# Get current robot position
x, y, theta = slam.getpos()
# Get current map bytes as grayscale
slam.getmap(mapbytes)
# Display map and robot pose, exiting gracefully if user closes it
if not viz.display(x/1000., y/1000., theta, mapbytes):
exit(0)
# Shut down the lidar connection
lidar.stop()
lidar.disconnect()
class LidarGraph(Thread):
def __init__(self, port):
Thread.__init__(self)
self.scan = None
self.running = False
self.port = port
def setup(self):
GPIO.output(23, GPIO.HIGH)
sleep(1)
self.lidar = RPLidar(self.port)
self.lidar.connect()
self.lidar.start_motor()
self.slam = RMHC_SLAM(LaserModel(),
MAP_SIZE_PIXELS,
MAP_SIZE_METERS,
map_quality=1,
max_search_iter=50)
self.mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
success = False
while success is not True:
try:
self.iterator = self.lidar.iter_scans()
next(self.iterator) #first scan is shit
success = True
except Exception as e:
print('retrying')
print(e)
sleep(0.5)
def restart(self):
self.stop()
self.go()
def go(self):
self.setup()
self.running = True
def stop(self):
print('Stopping')
self.running = False
self.lidar.stop_motor()
self.lidar.disconnect()
del self.lidar
GPIO.output(23, GPIO.LOW) # turn off lidar relay
sleep(1)
def update(self):
self.scan = next(self.iterator)
self.offsets = np.array([(np.radians(meas[1]), meas[2])
for meas in self.scan])
self.intens = np.array([meas[0] for meas in self.scan])
# BreezySLAM
previous_distances = None
previous_angles = None
items = [item for item in self.scan]
distances = [item[2] for item in items]
angles = [item[1] for item in items]
print(str(len(distances)) + ' distances')
# Update SLAM with current Lidar scan and scan angles if adequate
if len(distances) > MIN_SAMPLES:
print('updating slam')
self.slam.update(distances, scan_angles_degrees=angles)
previous_distances = distances.copy()
previous_angles = angles.copy()
# If not adequate, use previous
elif previous_distances is not None:
self.slam.update(previous_distances,
scan_angles_degrees=previous_angles)
# Get current robot position
x, y, theta = self.slam.getpos()
print('x ' + str(x) + 'y ' + str(y) + 'theta ' + str(theta))
# Get current map bytes as grayscale
self.slam.getmap(self.mapbytes)
sleep(0.05) #gather more samples?
def data(self):
if self.running != True:
return {'intens': [], 'offsets': []}
return {
'intens': self.intens.tolist(),
'offsets': self.offsets.tolist()
}
def get_map(self):
# slam map
return self.mapbytes
def run(self):
print('run')
iterator = None
while True:
if self.running:
try:
self.update()
print('Updated')
except Exception as e:
print('Exception during update')
print(e)
class Robot:
def __init__(self, messagequeue):
# Connect to Lidar unit
# when initializing the robot object, we first open lidar and start
# reading data from the lidar
self.lidar = lidarReader('/dev/mylidar', 115200)
self.mover = MotorController()
# Create an RMHC SLAM object with a laser model and optional robot model
self.slam = RMHC_SLAM(LaserModel(), MAP_SIZE_PIXELS, MAP_SIZE_METERS)
# Set up a SLAM display
self.viz = MapVisualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS, 'SLAM')
# Initialize empty map
self.mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
self.messages = messagequeue
# All the functions should run in the same process so that the variable can be shared
self.thread = threading.Thread(target=self.navigate, args=())
# the main-thread will exit without checking the sub-thread at the end of the main thread.
# At the same time, all sub-threads with the daemon value of True will end with the main thread, regardless of whether the operation is completed.
self.thread.daemon = True
self.navigating = True
self.thread.start()
# Construct Map should always run in the main process
def constructmap(self):
while True:
time.sleep(0.0001)
# Update SLAM with current Lidar scan, using first element of (scan, quality) pairs
self.slam.update(self.lidar.getdata())
#print(self.lidar.getdata())
# Get current robot position
x, y, theta = self.slam.getpos()
# Get current map bytes as grayscale
self.slam.getmap(self.mapbytes)
# Display map and robot pose, exiting gracefully if user closes it
if not self.viz.display(x / 1000., y / 1000., theta,
self.mapbytes):
break
def navigate(self):
while self.navigating:
time.sleep(
0.01) # (yield) allowing reading data from the serailport
if (
self.messages.empty()
): # The camera doesn't detect one traffic sign message.empty()
#if(True): # The camera doesn't detect one traffic sign message.empty()
front_too_close, left_too_close, right_too_close = False, False, False
if (self.lidar.angle_180 < 400):
front_too_close = True
left_angle = np.argmin(self.lidar.left_container)
right_angle = np.argmin(self.lidar.right_container)
if (self.lidar.left_container[left_angle] < 250):
left_too_close = True
if (self.lidar.right_container[right_angle] < 250):
right_too_close = True
if (front_too_close and left_too_close and right_too_close):
self.mover.backward()
elif (front_too_close):
if (self.lidar.angle_270 > self.lidar.angle_90):
self.mover.turn_left()
else:
self.mover.turn_right()
elif (left_too_close or right_too_close):
if (left_too_close):
self.mover.turn_right(left_angle)
else:
self.mover.turn_left(right_angle)
else:
self.mover.forward()
#print(self.lidar.left_container)
#print(front_too_close, left_too_close, right_too_close, self.lidar.angle_180)
else:
sign = self.messages.get() # get the detection id
if (sign == 1):
self.mover.stop()
elif (sign == 2):
self.mover.turn_right()
else:
self.mover.turn_left()
def run_slam(LIDAR_DEVICE, MAP_SIZE_PIXELS, MAP_SIZE_METERS, mapbytes,
posbytes, odometry, command, state, STATES):
MIN_SAMPLES = 200
# Connect to Lidar unit
lidar = start_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,
map_quality=_DEFAULT_MAP_QUALITY,
hole_width_mm=_DEFAULT_HOLE_WIDTH_MM,
random_seed=None,
sigma_xy_mm=_DEFAULT_SIGMA_XY_MM,
sigma_theta_degrees=_DEFAULT_SIGMA_THETA_DEGREES,
max_search_iter=_DEFAULT_MAX_SEARCH_ITER)
# Initialize empty map
mapbytes_ = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
# Send command to start scan
lidar.start_scan()
# We will use these to store previous scan in case current scan is inadequate
previous_distances = None
previous_angles = None
pose_change = (0, 0, 0)
pose_change_find = (0, 0, 0)
iter_times = 1
while command[0] != b'qa':
# read a scan
items = lidar.scan()
# Extract distances and angles from triples
distances = [item[2] for item in items]
angles = [item[1] for item in items]
if state[0] == STATES['stop']:
if command[0] == b'lm':
mapbytes_find = mapbytes
pose_change_find = find_slam_pos(lidar, MAP_SIZE_PIXELS,
MAP_SIZE_METERS,
mapbytes_find,
pose_change_find,
iter_times / 2.0, slam)
iter_times = iter_times + 1.0
print(pose_change_find)
pose_change = pose_change_find
pose_change_find = (0, 0, 0)
'''
items = lidar.scan()
# Extract distances and angles from triples
distances = [item[2] for item in items]
angles = [item[1] for item in items]
slam.update(distances, pose_change, scan_angles_degrees=angles)
'''
if (iter_times > 10):
command[0] = b'w'
print(slam)
slam.setmap(mapbytes_find)
pose_change = pose_change_find
pose_change_find = (0, 0, 0)
print('ennnnnnnnnnnnnnnnnnd')
print(pose_change)
iter_times = 1
else:
# Update SLAM with current Lidar scan and scan angles if adequate
if len(distances) > MIN_SAMPLES:
slam.update(distances, pose_change, scan_angles_degrees=angles)
previous_distances = distances.copy()
previous_angles = angles.copy()
# If not adequate, use previous
elif previous_distances is not None:
slam.update(previous_distances,
pose_change,
scan_angles_degrees=previous_angles)
#pass the value into point_cal function
global point_a, point_b
global current_theta_o
# Get current robot position
x, y, theta = slam.getpos()
current_theta_o = theta
point_a = x - 5000
point_b = y - 5000
# Calculate robot position in the map
xbytes = int(x / 1000. / MAP_SIZE_METERS * MAP_SIZE_PIXELS)
ybytes = int(y / 1000. / MAP_SIZE_METERS * MAP_SIZE_PIXELS)
# Update robot position
posbytes[xbytes + ybytes * MAP_SIZE_PIXELS] = 255
#print('LIDAR::', command[0], x,y,theta)
pose_change = (odometry[0], odometry[1] / np.pi * 180, odometry[2])
# Get current map bytes as grayscale
slam.getmap(mapbytes_)
# Update map
mapbytes[:] = mapbytes_
# Shut down the lidar connection
lidar.stop()
lidar.disconnect()
print('LIDAR::thread ends')
class LidarProcessingNode(Node):
def __init__(self, visualize):
super().__init__('cont')
self.visualize = visualize
self.slam = RMHC_SLAM(LaserModel(), MAP_SIZE_PIXELS, MAP_SIZE_METERS)
if self.visualize:
self.viz = MapVisualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS, 'SLAM')
self.subscriber = self.create_subscription(LaserScan, '/scan',
self.lidarCallback, 10)
self.mapPublisher = self.create_publisher(OccupancyGrid, '/map', 1)
self.locPublisher = self.create_publisher(Pose, '/pose', 1)
print("Setup Finished")
self.previous_distances = None
self.previous_angles = None
self.mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
self.i = True
def lidarCallback(self, data):
# print("Received Lidar Message")
self.angle_min = data.angle_min
self.angle_max = data.angle_max
self.angle_increment = data.angle_increment
self.time_increment = data.time_increment
self.scan_time = data.scan_time
self.range_max = data.range_max
self.range_min = data.range_min
self.intensities = data.intensities
self.distances = list(data.ranges)
self.angle_max = self.angle_max - self.angle_min
self.angle_min = 0.0
#Filtering out points that are outside the max range or Nan.
self.angles = [
i * 180.0 / np.pi for i in np.arange(
self.angle_min, self.angle_max, self.angle_increment)
] + [self.angle_max * 180.0 / np.pi]
self._distances = [i * 1000.0 for i in self.distances]
self._distances, self.angles = (list(t) for t in zip(
*[x for x in zip(self._distances, self.angles) if x[0] <= 5000]))
# print(max(filteredDistances))
# print(len(filteredDistances))
# print(len(filteredAngles))
# if len(self.angles) != len(self.distances):
# self.angles = self.angles[:len(self.distances)]
# print(len(self.distances)
# print(self.angles)
if len(self._distances) > MIN_SAMPLES:
self.slam.update(self._distances, scan_angles_degrees=self.angles)
self.previous_distances = self._distances.copy()
self.previous_angles = self.angles.copy()
# If not adequate, use previous
elif previous_distances is not None:
self.slam.update(self.previous_distances,
scan_angles_degrees=self.previous_angles)
# self.slam.update(laserScanData)
x, y, theta = self.slam.getpos()
self.slam.getmap(self.mapbytes)
if self.visualize:
if not self.viz.display(x / 1000., y / 1000., theta,
self.mapbytes):
exit(0)
#Publishing Position data
pose = Pose()
pose.position.x = x
pose.position.y = y
pose.orientation.z = theta
self.locPublisher.publish(pose)
#Publishing Map data
map = OccupancyGrid()
map.header.stamp.sec = 0
map.header.stamp.nanosec = 0
map.header.frame_id = "1"
map.info.map_load_time.sec = 0
map.info.map_load_time.nanosec = 0
map.info.resolution = MAP_SIZE_PIXELS / MAP_SIZE_METERS
map.info.width = MAP_SIZE_PIXELS
map.info.height = MAP_SIZE_PIXELS
map.info.origin.position.x = 5000.0
map.info.origin.position.y = 5000.0
mapimg = np.reshape(np.frombuffer(self.mapbytes, dtype=np.uint8),
(MAP_SIZE_PIXELS, MAP_SIZE_PIXELS))
norm_const = 100 / mapimg.max()
norm_map = (np.absolute(mapimg.astype(float) * norm_const -
100.0)).astype(int)
# plt.matshow(norm_map)
# plt.show()
map.data = norm_map.flatten().tolist()
self.mapPublisher.publish(map)
class JetsonEV(object):
xbox_mode = 'xbox'
xbox_direct_mode = 'xbox_direct'
xbox_forwarding_mode = 'xbox_forwarding'
__running_car__ = None
VESC_CONTROL = 'VESC_CONTROL'
ARDUINO_CONTROL = 'ARDUINO_CONTROL'
# Default TCP ports for out-going data
SPEED_PORT = 4020
"""Constant: TCP port being used to send speed measurement"""
IMU_PORT = 5025
"""Constant: TCP port being used to send IMU measurements"""
LIDAR_PORT = 5026
"""Constant: TCP port being used to send lidar measurements"""
CAMERA_PORT = 5027
"""Constant: TCP port being used to send camera frames"""
SLAM_PORT = 5028
"""Constant: TCP port being used to send SLAM data (x, y, theta, map)"""
ARDUINO_CONFIG = {
'steering_pin': 9,
'motor_pwm_pin': 10,
'motor_pin_a': 19,
'motor_pin_b': 18,
'motor_pin_c': 2
}
"""Constant: Dictionary of pin configuration to use for arduino control. Default values are good to use for a MEGA.
These values may be changed before constructing JetsonEV.
* * *
"""
def __init__(self,
mode='xbox',
initialize_imu=True,
imu_bus=0,
initialize_camera=False,
initialize_lidar=False,
slam_map_meters=15,
slam_map_pixels=1000,
max_speed_limit=5,
max_duty_cycle=0.2,
rc_control='VESC_CONTROL',
rc_communication='/dev/ttyTHS1'):
"""
:param mode: Control mode for the vehicle. Possible values are:
>- JetsonEV.xbox_mode
>- JetsonEV.xbox_direct_mode
>- JetsonEV.xbox_forwarding_mode
>See [here](./modes.html) for more details on each mode.
:param initialize_imu: Whether or not to initialize imu
:param imu_bus: I2C bus number to communicate with the IMU
:param initialize_camera: Whether or not to initialize camera
:param initialize_lidar: Whether or not to initialize rplidar
:param slam_map_meters: The distance in meters to use for the SLAM map size. A square map is
made using this distance for each side.
:param slam_map_pixels: The SLAM square map size in pixels. This controls the resolution of the map.
:param max_speed_limit: Max speed in m/s when using speed controller.
:param max_duty_cycle: Max duty cycle to send when using direct duty cycle control.
:param rc_control: Type of hardware control for rc components (motor and steering servo). Can be either
JetsonEV.VESC_CONTROL or JetsonEV.ARDUINO_CONTROL.
:param rc_communication: Port or bus to use for rc_control. If using VESC, this should point to the UART port.
If using USB for UART control, set rc_communication = 'USB' (linux only).
If rc_control=JetsonEV.ARDUINO, this should be an integer for the I2C bus number.
* * *
"""
JetsonEV.__running_car__ = self
self.rc_control = rc_control
self._timed_tasks = []
"""private list to hold all existing TimedTask objects"""
self._output_sockets = []
"""private list to hold all existing output TCP sockets objects"""
self.max_speed_limit = max_speed_limit
self.duty_cycle_limit = max_duty_cycle
self._speed = [0]
self._speed_lock = threading.Lock()
'''************** Initiate Steering and Motor ****************'''
try:
self._initialize_rc_control(rc_communication)
except OSError as e:
print(e)
'''*********************************************************'''
'''****************** Initiate Camera **********************'''
self.camera = None
if initialize_camera:
print('initiating Camera... ', end='')
try:
camera = Camera(flip=2)
self._latest_frame = [0]
self._latest_frame_lock = threading.Lock()
self.camera_socket = SendSocket(tcp_port=JetsonEV.CAMERA_PORT,
tcp_ip=ip_address)
self._output_sockets.append(self.camera_socket)
def update_frame(frame):
self.latest_frame = frame
self.camera_socket.send_data(frame)
# wrap the object in a TimedTask set to update at fixed rate
self.camera = self.add_timed_task(
calling_function=camera.read,
object_to_wrap=camera,
forwarding_function=update_frame,
sampling_rate=120)
except Exception as e:
print(e)
print('success')
'''*********************************************************'''
'''******************* Initiate IMU ************************'''
self.imu: TimedTask
if initialize_imu:
try:
imu = MPU6050(bus=imu_bus,
device_addr=0x68) # create the imu device
print('Successfully connected to IMU')
self._imu_values = [0]
self._imu_values_lock = threading.Lock()
# wrap the object in a TimedTask set to update at fixed rate
self.imu = self.add_timed_task(
calling_function=self._get_imu_values,
object_to_wrap=imu,
sampling_rate=50)
self.imu_socket = SendSocket(tcp_port=JetsonEV.IMU_PORT,
tcp_ip=ip_address)
self._output_sockets.append(self.imu_socket)
imu_config_dict = get_calibration()
self._imu_rotation = np.array(
imu_config_dict['rotation_matrix'])
self._gravity_offset = imu_config_dict['gravity']
self._gyro_offset = np.array(imu_config_dict['gyro_offsets'])
except Exception as e:
print(e)
print('ERROR: Could not connect to IMU')
self.imu = None
if hasattr(self, 'imu_socket'):
self._output_sockets.remove(self.imu_socket)
'''*********************************************************'''
'''****************** Initiate Lidar ***********************'''
self.lidar = None
self._theta_lock = threading.Lock()
self._theta = 0
self._x_lock = threading.Lock()
self._x = 0
self._y_lock = threading.Lock()
self._y = 0
self._map_lock = threading.Lock()
self._map = bytearray(slam_map_pixels * slam_map_pixels)
self._breezyslam = RMHC_SLAM(LaserModel(),
map_size_meters=slam_map_meters,
map_size_pixels=slam_map_pixels,
hole_width_mm=500)
if initialize_lidar:
started = threading.Event(
) # threading event to watch if lidar started
# This is done to prevent the script from locking up if the lidar needs to be restarted
def check_lidar():
start_time = time.time()
while not started.isSet():
time.sleep(0.2)
if time.time() - start_time > 10:
os.kill(os.getpid(), signal.SIGTERM)
raise Exception('Unable to start Lidar.')
initialize_thread = threading.Thread(target=check_lidar)
initialize_thread.start()
try:
print('connecting to lidar...', end='')
lidar = RPLidar(scan_mode=0)
started.set()
try:
initialize_thread.join()
except Exception as e:
raise e
except Exception as e:
print(e)
print('ERROR: Unable to connect to lidar sensor.')
os.kill(os.getpid(), signal.SIGTERM)
self._latest_lidar_scan = lidar.get_scan_as_xy(filter_quality=True)
self._latest_lidar_scan_lock = threading.Lock()
print('success')
self.lidar_socket = SendSocket(tcp_port=JetsonEV.LIDAR_PORT,
tcp_ip=ip_address)
self._output_sockets.append(self.lidar_socket)
self.slam_socket = SendSocket(tcp_port=JetsonEV.SLAM_PORT,
tcp_ip=ip_address)
self._output_sockets.append(self.slam_socket)
self._update_map = [True]
self._save_criteria_dist = 100 # must move ** mm before updating map
self._save_criteria_angle = 20 # must rotate more than ** deg before updating map
def update_scan(scan):
scan = np.array(scan)
self.latest_lidar_scan = scan
self._breezyslam.update(scan[:, 1].tolist(),
scan_angles_degrees=scan[:,
0].tolist(),
should_update_map=self._update_map[0])
x, y, theta = self._breezyslam.getpos()
dist = np.sqrt((x - self.x)**2 + (y - self.y)**2)
del_theta = abs(theta - self.theta)
if dist > self._save_criteria_dist or del_theta > self._save_criteria_angle:
self._update_map[0] = True
else:
self._update_map[0] = False
self.x = x
self.y = y
self.theta = theta
with self._map_lock:
self._breezyslam.getmap(self._map)
self.lidar_socket.send_data(scan)
self.slam_socket.send_data({
'x':
self.x,
'y':
self.y,
'theta':
self.theta,
'map':
np.frombuffer(self.map, dtype=np.uint8),
'meters':
slam_map_meters,
'pixels':
slam_map_pixels
})
self.lidar = self.add_timed_task(
calling_function=lidar.get_scan_as_vectors,
calling_func_args=[True],
object_to_wrap=lidar,
forwarding_function=update_scan,
sampling_rate=5)
print("initializing SLAM map")
for _ in range(10):
scan = np.array(lidar.get_scan_as_vectors(True))
self._breezyslam.update(scan[:, 1].tolist(),
scan_angles_degrees=scan[:,
0].tolist(),
should_update_map=True)
time.sleep(0.05)
'''*********************************************************'''
'''************ Initiate XBOX Controller *******************'''
self.xbox_controller: Xbox360Controller
self._initialize_xbox_controller(mode)
'''*********************************************************'''
'''***************** Setup speed control *******************'''
self._desired_speed = [0]
self.last_error = 0
self._current_duty = 0
self._integrated_speed_error = 0
self._desired_speed_lock = threading.Lock()
self._speed_control_task = TimedTask(
calling_function=self._speed_control, sampling_rate=20)
if mode in [JetsonEV.xbox_mode]:
# define an in between function to convert percentage from xbox controller to speed using max speed limit.
def percent_to_speed(percent):
self.set_motor_speed(percent * self.max_speed_limit)
self._xbox_speed_func = percent_to_speed
self.duty_cycle_limit = 1
self.start_speed_control()
elif mode in [JetsonEV.xbox_direct_mode]:
self._xbox_speed_func = self.set_motor_percent
'''*********************************************************'''
print('Starting Vehicle...', end='')
self.start_sockets()
self.start_all_tasks()
print('running.')
def start_speed_control(self):
"""Starts the internal speed controller. Speed is controlled with set_motor_speed(). This may be called
automatically depending on mode.
* * *
"""
self._speed_control_task.start()
def stop_speed_control(self):
"""Stops the internal speed controller.
* * *
"""
self._speed_control_task.stop()
def _speed_control(self):
# NOT PROPER (OR AT LEAST GOOD) IMPLEMENTATION OF PID CONTROL
if abs(self.speed) < 0.4 and abs(self.desired_speed) < 0.5:
self.set_motor_percent(0)
self._current_duty = 0
return
error = self.desired_speed - self.speed
error_rate = error - self.last_error
self.last_error = error
self._integrated_speed_error += error
self._current_duty = self._current_duty + \
error * gains[self.gain_id]['p'] + \
error_rate * gains[self.gain_id]['d'] + \
self._integrated_speed_error * gains[self.gain_id]['i']
# print("speed: {:.3f}, desired: {:.3f}, duty:{:.3f}".format(self.speed, self.desired_speed, self._current_duty))
self.set_motor_percent(self._current_duty)
@property
def x(self):
with self._x_lock:
return self._x
@x.setter
def x(self, new_x):
with self._x_lock:
self._x = new_x
@property
def y(self):
with self._y_lock:
return self._y
@y.setter
def y(self, new_y):
with self._y_lock:
self._y = new_y
@property
def theta(self):
with self._theta_lock:
return self._theta
@theta.setter
def theta(self, new_theta):
with self._theta_lock:
self._theta = new_theta
@property
def map(self):
with self._map_lock:
return self._map.copy()
@property
def desired_speed(self):
""" The currently stored desired speed (m/s)
* * *
"""
with self._desired_speed_lock:
return self._desired_speed[0]
@desired_speed.setter
def desired_speed(self, new_speed):
with self._desired_speed_lock:
self._desired_speed[0] = new_speed
@property
def speed(self):
""" Holds the current vehicle speed measurement.
* * *"""
with self._speed_lock:
return self._speed[0]
@speed.setter
def speed(self, new_speed):
with self._speed_lock:
self._speed[0] = new_speed
@property
def latest_lidar_scan(self):
""" Holds the latest complete lidar scan.
* * *"""
with self._latest_lidar_scan_lock:
return self._latest_lidar_scan
@latest_lidar_scan.setter
def latest_lidar_scan(self, scan):
with self._latest_lidar_scan_lock:
self._latest_lidar_scan = scan
@property
def latest_frame(self):
""" Holds the latest camera image.
* * *"""
with self._latest_frame_lock:
return self._latest_frame[0]
@latest_frame.setter
def latest_frame(self, frame):
with self._latest_frame_lock:
self._latest_frame[0] = frame
@property
def imu_values(self):
""" Holds the latest imu values.
* * *"""
with self._imu_values_lock:
return self._imu_values[0]
@imu_values.setter
def imu_values(self, values):
with self._imu_values_lock:
self._imu_values[0] = values
def _initialize_rc_control(self, communication):
"""This function creates the properties to control the motor and steering.
:param communication: either 'VESC_CONTROL' for using the VESC or 'ARDUINO_CONTROL' for using an ArduinoI2CBus
* * *
"""
if self.rc_control == self.VESC_CONTROL:
self.gain_id = JetsonEV.VESC_CONTROL
if communication == 'USB':
try:
for dev in os.listdir('/dev/serial/by-id'):
if dev.find('ChibiOS') >= 0:
communication = '/dev/serial/by-id/' + dev
except FileNotFoundError:
raise Exception(
'Unable to find VESC device. Check port and power.')
try:
self.vesc = VESC(serial_port=communication)
except Exception as e:
raise e
self._set_servo_percent = self.vesc.set_servo
def vesc_motor_duty(percentage):
# percentage should be between -1 and 1
self.vesc.set_duty_cycle(percentage)
self._set_motor_duty = vesc_motor_duty
self._speed_socket = SendSocket(tcp_port=JetsonEV.SPEED_PORT,
tcp_ip=ip_address)
self._output_sockets.append(self._speed_socket)
new_conversion = speed_conversion * poles_on_motor # poles in motor
def set_motor_speed(new_speed):
self.speed = new_speed * new_conversion
self._speed_socket.send_data(self.speed)
self.motor_speed_task = self.add_timed_task(
calling_function=self.vesc.get_rpm,
sampling_rate=20,
forwarding_function=set_motor_speed,
verbose=False)
else: # Use arduino
self.gain_id = JetsonEV.ARDUINO_CONTROL
self.arduino_devices = ArduinoI2CBus(communication)
self.StWhl = self.arduino_devices.create_servo_dev(
pin=JetsonEV.ARDUINO_CONFIG['steering_pin'])
self.StWhl.attach()
self._set_servo_percent = self.StWhl.write_percentage
self.Motor = self.arduino_devices.create_servo_dev(
pin=JetsonEV.ARDUINO_CONFIG['motor_pwm_pin'])
self.Motor.attach()
self._last_duty = [0]
self._was_reversing = [False]
# define function to handle a percentage from -1 to 1 and convert to regular ESC percentage
def arduino_motor_duty(percentage):
if percentage < 0 and self._last_duty[
0] >= 0 and not self._was_reversing[0]:
self.Motor.write_percentage(.1)
time.sleep(0.15)
self.Motor.write_percentage(.5)
self._current_duty = 0
time.sleep(0.1)
self._was_reversing[0] = True
self._last_duty[0] = percentage
if percentage >= 0:
if percentage > 0:
self._was_reversing[0] = False
percentage = 0.5 * percentage + 0.5 # convert to percent of pwm above 50 %
else:
percentage = 0.5 - 0.5 * abs(percentage)
self.Motor.write_percentage(percentage)
self._set_motor_duty = arduino_motor_duty
encoder = self.arduino_devices.create_BLDC_encoder_dev(
pin_A=JetsonEV.ARDUINO_CONFIG['motor_pin_a'],
pin_B=JetsonEV.ARDUINO_CONFIG['motor_pin_b'],
pin_C=JetsonEV.ARDUINO_CONFIG['motor_pin_c'])
self._speed_socket = SendSocket(tcp_port=JetsonEV.SPEED_PORT,
tcp_ip=ip_address)
self._output_sockets.append(self._speed_socket)
def set_motor_speed(new_speed):
self.speed = new_speed * speed_conversion
self._speed_socket.send_data(self.speed)
self.motor_encoder = self.add_timed_task(
calling_function=encoder.get_speed,
object_to_wrap=encoder,
sampling_rate=20,
forwarding_function=set_motor_speed,
verbose=True)
def shutdown(self):
"""Shuts down the car. It is necessary to call this before this object goes out of scope in order to shut down
the hardware properly.
* * *
"""
self.stop_speed_control()
self.set_motor_percent(0)
try:
print('shutting down controller')
self.xbox_controller.close()
except AttributeError as e:
print(e)
if hasattr(self, 'vesc'):
self.vesc.set_duty_cycle(0)
self.vesc.stop_heartbeat()
if hasattr(self, 'arduino_devices'):
print('shutting down arduino')
self.arduino_devices.clear_all_devices()
print('closing ports')
[x.stop() for x in self._output_sockets]
print('stopping any tasks')
[x.stop() for x in self._timed_tasks]
if self.lidar is not None:
self.lidar.wrapped_object.stopmotor()
print('shutting down camera')
self.close_camera()
def start_sockets(self):
""" Starts all the TCP sockets in _output_sockets. This is called automatically in the constructor.
* * *"""
[x.thread.start() for x in self._output_sockets]
def _connect_controller(self):
try:
self.xbox_controller = Xbox360Controller(index=1,
axis_threshold=0.05)
except Exception as e: # this library emits a stupid general exception that makes it difficult to retry
try:
self.xbox_controller = Xbox360Controller(index=2,
axis_threshold=0.05)
except Exception as e:
self.xbox_controller = Xbox360Controller(index=0,
axis_threshold=0.05)
def _initialize_xbox_controller(self, mode):
if mode in [self.xbox_mode, self.xbox_direct_mode]:
self._connect_controller()
self.xbox_controller.axis_l.when_moved = self._xbox_adjust_steering
self.xbox_controller.trigger_r.when_moved = self._xbox_control_speed
self.xbox_controller.trigger_l.when_moved = self._xbox_control_speed
print('sucessfully connected to controller')
elif mode == self.xbox_forwarding_mode:
self._connect_controller()
print('sucessfully connected to controller.')
print(
'Don\'t forget to set forwarding functions using: set_trigger_l_func, set_trigger_r_func,'
' set_l_joystick_func')
def set_trigger_l_func(self, function):
"""
Use this function when in xbox_forwarding_mode to assign a function to handle a signal from the left trigger.
:param function: function that takes one argument that will be an Axis object from the xbox360controller
* * *
"""
self.xbox_controller.trigger_l.when_moved = function
def set_trigger_r_func(self, function):
"""
Use this function when in xbox_forwarding_mode to assign a function to handle a signal from the right trigger.
:param function: function that takes one argument that will be an Axis object from the xbox360controller
* * *
"""
self.xbox_controller.trigger_r.when_moved = function
def set_l_joystick_func(self, function):
"""
Use this function when in xbox_forwarding_mode to assign a function to handle a signal from the left joystick.
:param function: function that takes one argument that will be an Axis object from the xbox360controller
* * *
"""
self.xbox_controller.axis_l.when_moved = function
def set_r_joystick_func(self, function):
"""
Use this function when in xbox_forwarding_mode to assign a function to handle a signal from the right joystick.
:param function: function that takes one argument that will be an Axis object from the xbox360controller
* * *
"""
self.xbox_controller.axis_r.when_moved = function
def _xbox_adjust_steering(self, axis):
self.set_steering_percent(axis.x)
def _xbox_control_speed(self, axis):
trigger_value = axis.value
if axis.name == "trigger_r":
speed_percent = trigger_value
else:
speed_percent = -trigger_value
self._xbox_speed_func(speed_percent)
def set_motor_speed(self, new_speed):
""" The value set here will be assigned to self.desired_speed. This is only effective in xbox_mode or if the
internal speed controller is turned on with start_speed_control.
:param new_speed: new speed in m/s
* * *
"""
# if self.rc_control == JetsonEV.VESC_CONTROL:
# self.vesc.set_rpm(int(new_speed/(speed_conversion*poles_on_motor)))
# else:
self.desired_speed = new_speed
def set_motor_percent(self, percentage):
""" This function will directly send a pwm signal to the motor controller. Do not use this while the internal
speed control is running (i.e. DO NOT use while in xbox_mode or if start_speed_control() has been called).
:param percentage: motor duty cycle to use between -1 (reverse) and 1 (forward)
* * *
"""
if percentage > 1:
percentage = 1
elif percentage < -1:
percentage = -1
# print(percentage*self.duty_cycle_limit)
self._set_motor_duty(percentage * self.duty_cycle_limit)
def set_steering_percent(self, percentage):
"""
:param percentage: steering servo percentage between -1 and 1
* * *
"""
if percentage > 1:
percentage = 1
elif percentage < -1:
percentage = -1
steering_percent = -(percentage - 1) / 2
self._set_servo_percent(steering_percent)
def close_camera(self):
if self.camera is not None:
self.camera.wrapped_object.release()
def _get_imu_values(self):
self.imu_values = (
self._imu_rotation @ self.imu.wrapped_object.accel.
xyz, # + [0, 0, self._gravity_offset],
self._imu_rotation
@ (self.imu.wrapped_object.gyro.xyz - self._gyro_offset))
# , self._imu_rotation @ self.imu.wrapped_object.mag.xyz)
self.imu_socket.send_data({
'accel': self.imu_values[0],
'gyro': self.imu_values[1]
})
return self.imu_values
def get_x(self):
"""
:returns Latest x position measurement in millimeters calculated from SLAM.
* * *
"""
return self.x
def get_y(self):
"""
:returns Latest y position measurement in millimeters calculated from SLAM.
* * *
"""
return self.y
def get_yaw(self):
"""
:returns Latest yaw angle measurement in degrees calculated from SLAM.
* * *
"""
return self.theta
def get_imu_values(self):
"""
:returns Latest measurement from the IMU.
* * *
"""
return self.imu_values
def get_lidar_values(self):
"""
:returns: Latest measurement from the Lidar sensor.
* * *"""
return self.latest_lidar_scan
def get_camera_frame(self):
"""
:returns: Latest camera frame.
* * *
"""
return self.latest_frame
def get_speed(self):
"""
:returns: Latest speed measurement (m/s)
* * *"""
def add_timed_task(self,
calling_function,
object_to_wrap=None,
sampling_rate=10,
calling_func_args=[],
forwarding_function=None,
verbose=False):
"""
This is a helper function to create a TimedTask object. The only difference between using this and making a
TimedTask directly, is that the resulting new task is also added to JetsonEV._timed_tasks which are all
automatically stopped when shutdown() is called.
:param calling_function: The function that will be called at the set interval.
:type calling_function: function
:param object_to_wrap: An instance of an object that is associated with the repetitive task. References to
this object can be obtained from two member attributes of TimedTask under the name "wrapped_object" and
the name of the type of the object that is passed (i.e. if an object of type "Sensor" is passed, then the
reference would be "myTimedTask.Sensor"). This can be set as None if you do not want this feature or are
not using an object.
:type object_to_wrap: object
:param sampling_rate: The frequency in Hz of task loop.
:type sampling_rate: float
:param calling_func_args: A list of any arguments that need to be passed to the calling_function when it is
called.
:type calling_func_args: list
:param forwarding_function: An additional optional function that can be specified to be called every loop.
:type forwarding_function: function
:param verbose: Whether or not to print errors from the task (i.e. task is not keeping up to desired run speed).
:returns The created TimedTask object.
* * *
"""
new_task = TimedTask(calling_function,
object_to_wrap,
sampling_rate,
calling_func_args,
forwarding_function,
verbose=verbose)
self._timed_tasks.append(new_task)
return new_task
def start_all_tasks(self):
"""Starts all the created TimedTasks that are being tracked by JetsonEV. This is called in the constructor.
* * *
"""
[x.start() for x in self._timed_tasks]
raw_data = json.loads(socket1.recv())
raw_data_new = [[i * 1000 for i in raw_data[0:360]]]
for i in range(360, (len(raw_data))):
fixed_data_org = [k * 1000 for k in raw_data[i]]
raw_data_new.append(fixed_data_org)
raw_data_new = list(raw_data_new)[0]
# Sends message back to raw data script
socket1.send_string('Got the raw data: %s ' % raw_data_new)
# Update SLAM with current Lidar scan
slam.update(raw_data_new)
# Get current robot position
map_x, map_y, map_theta = slam.getpos()
#print(x, y)
# Get current map bytes as grayscale
slam.getmap(mapbytes)
# Convert byteArray to array containing values 0-255 indicating grayscale colors
mapping = np.reshape(np.frombuffer(mapbytes, dtype=np.uint8),
(MAP_SIZE_PIXELS, MAP_SIZE_PIXELS))
# Convert array containing grayscale colors to 0's for open space and 1's for obstacles
obstacleMap = convertToObstacleMap(mapping)
# RUN STEERING ALGORITHM TO PASS THROUGH PATH
# Display map and robot pose, exiting gracefully if user closes it
if not viz.display(map_x / 1000., map_y / 1000., map_theta, mapbytes):
class narlam:
def __init__(self):
self.flag = 0
self.lidar = Lidar(LIDAR_DEVICE)
self.slam = RMHC_SLAM(LaserModel(), MAP_SIZE_PIXELS, MAP_SIZE_METERS)
self.viz = MapVisualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS,
'SLAM MAP') # no visualizer needed
self.trajectory = []
self.mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
self.iterator = self.lidar.iter_scans()
self.previous_distances = None
self.previous_angles = None
self.x = 0.0
self.y = 0.0
self.theta = 0.0
def slam_no_map(self, path_map_name):
# doing slam with building maps from zero simultaneously
next(self.iterator)
while True:
if self.flag == 1:
break
items = [item for item in next(self.iterator)]
distances = [item[2] for item in items]
angles = [item[1] for item in items]
if len(distances) > MIN_SAMPLES:
self.slam.update(distances, scan_angles_degrees=angles)
self.previous_distances = distances.copy()
self.previous_angles = angles.copy()
elif self.previous_distances is not None:
self.slam.update(self.previous_distances,
scan_angles_degrees=self.previous_angles)
self.x, local_y, local_theta = self.slam.getpos()
local_theta = local_theta % 360
if local_theta < 0:
self.theta = 360 + local.theta
else:
self.theta = local_theta
self.slam.getmap(self.mapbytes)
# save map generated by slam
image = Image.frombuffer('L', (MAP_SIZE_PIXELS, MAP_SIZE_PIXELS),
self.mapbytes, 'raw', 'L', 0, 1)
image.save(path_map_name)
self.lidar.stop()
self.lidar.disconnect()
def slam_yes_map(self, path_map_name):
# doing localization only, with pre-built map image file.
with open(path_map_name, "rb") as map_img:
f = map_img.read()
b = bytearray(f)
self.slam.setmap(b)
next(self.iterator)
while True:
if self.flag == 1:
break
items = [item for item in next(self.iterator)]
distances = [item[2] for item in items]
angles = [item[1] for item in items]
if len(distances) > MIN_SAMPLES:
self.slam.update(distances,
scan_angles_degrees=angles,
should_update_map=False)
self.previous_distances = distances.copy()
self.previous_angles = angles.copy()
elif self.previous_distances is not None:
self.slam.update(self.previous_distances,
scan_angles_degrees=self.previous_angles,
should_update_map=False)
self.x, self.y, self.theta = self.slam.getpos()
self.lidar.stop()
self.lidar.disconnect()
async def main():
async with websockets.connect(SERVER_URL,
max_size=None,
max_queue=None,
ping_interval=None,
ping_timeout=None) as websocket:
# 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)
# Initialize an empty trajectory
trajectory = []
# Initialize empty map
mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
# Create an iterator to collect scan data from the RPLidar
iterator = lidar.iter_scans()
# We will use these to store previous scan in case current scan is inadequate
previous_distances = None
previous_angles = None
# First scan is crap, so ignore it
next(iterator)
while True:
# Extract (quality, angle, distance) triples from current scan
items = [item for item in next(iterator)]
# Extract distances and angles from triples
distances = [item[2] for item in items]
angles = [item[1] for item in items]
# Update SLAM with current Lidar scan and scan angles if adequate
if len(distances) > MIN_SAMPLES:
slam.update(distances, scan_angles_degrees=angles)
previous_distances = distances.copy()
previous_angles = angles.copy()
# If not adequate, use previous
elif previous_distances is not None:
slam.update(previous_distances, scan_angles_degrees=previous_angles)
# Get current robot position
x, y, theta = slam.getpos()
# Get current map bytes as grayscale
slam.getmap(mapbytes)
while True:
try:
await websocket.send(struct.pack(STRUCT_FORMAT, x, y, theta, mapbytes))
except:
else:
break
# Shut down the lidar connection
lidar.stop()
lidar.disconnect()
def main():
# Bozo filter for input args
if len(argv) < 3:
print('Usage: %s <dataset> <use_odometry> [random_seed]' % argv[0])
print('Example: %s exp2 1 9999' % argv[0])
exit(1)
# Grab input args
dataset = argv[1]
use_odometry = True if int(argv[2]) else False
seed = int(argv[3]) if len(argv) > 3 else 0
# Load the data from the file, ignoring timestamps
_, lidars, odometries = load_data('.', dataset)
# Build a robot model if we want odometry
robot = Rover() if use_odometry else None
# Create a CoreSLAM object with laser params and optional robot object
slam = RMHC_SLAM(MinesLaser(), MAP_SIZE_PIXELS, MAP_SIZE_METERS, random_seed=seed) \
if seed \
else Deterministic_SLAM(MinesLaser(), MAP_SIZE_PIXELS, MAP_SIZE_METERS)
# Report what we're doing
nscans = len(lidars)
print('Processing %d scans with%s odometry / with%s particle filter...' % \
(nscans, \
'' if use_odometry else 'out', '' if seed else 'out'))
progbar = ProgressBar(0, nscans, 80)
# Start with an empty trajectory of positions
trajectory = []
# Start timing
start_sec = time()
# Loop over scans
for scanno in range(nscans):
if use_odometry:
# Convert odometry to velocities
velocities = robot.computePoseChange(odometries[scanno])
# Update SLAM with lidar and velocities
slam.update(lidars[scanno], velocities)
else:
# Update SLAM with lidar alone
slam.update(lidars[scanno])
# Get new position
x_mm, y_mm, theta_degrees = slam.getpos()
# Add new position to trajectory
trajectory.append((x_mm, y_mm))
# Tame impatience
progbar.updateAmount(scanno)
stdout.write('\r%s' % str(progbar))
stdout.flush()
# Report elapsed time
elapsed_sec = time() - start_sec
print('\n%d scans in %f sec = %f scans / sec' %
(nscans, elapsed_sec, nscans / elapsed_sec))
# Create a byte array to receive the computed maps
mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
# Get final map
slam.getmap(mapbytes)
# Pickle the map to a file
pklname = dataset + '.map'
print('Writing map to file ' + pklname)
pickle.dump(mapbytes, open(pklname, 'wb'))
def slamThread():
global SLAMrot
global SLAMvel
# 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
viz = MapVisualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS, 'SLAM')
# Initialize an empty trajectory
trajectory = []
# Initialize empty map
mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
# Create an iterator to collect scan data from the RPLidar
iterator = lidar.iter_scans()
# We will use these to store previous scan in case current scan is inadequate
previous_distances = None
previous_angles = None
# First scan is crap, so ignore it
next(iterator)
# start time
start_time = time.time()
prevTime = start_time
print("start")
while True:
# Extract (quality, angle, distance) triples from current scan
items = [item for item in next(iterator)]
# Extract distances and angles from triples
distances = [item[2] for item in items]
angles = [item[1] for item in items]
# Update SLAM with current Lidar scan and scan angles if adequate
if len(distances) > MIN_SAMPLES:
slam.update(distances, pose_change = ( (SLAMvel, SLAMrot, time.time() - prevTime)),scan_angles_degrees=angles)
prevTime = time.time()
previous_distances = copy.copy(distances)
previous_angles = copy.copy(angles)
print("updated - if")
print((SLAMvel, SLAMrot, time.time() - prevTime))
# If not adequate, use previous
elif previous_distances is not None:
slam.update(previous_distances, pose_change = ( (SLAMvel, SLAMrot, time.time() - prevTime)),scan_angles_degrees=previous_angles)
prevTime = time.time()
print("updated - else")
print((SLAMvel, SLAMrot, time.time() - prevTime))
# Get current robot position
x, y, theta = slam.getpos()
# Add new position to trajectory
trajectory.append((x, y))
# Get current map bytes as grayscale
slam.getmap(mapbytes)
if(time.time() - start_time > 30):
# Put trajectory into map as black pixels
for coords in trajectory:
x_mm, y_mm = coords
x_pix = mm2pix(x_mm)
y_pix = mm2pix(y_mm)
mapbytes[y_pix * MAP_SIZE_PIXELS + x_pix] = 0;
pgm_save('ok.pgm', mapbytes, (MAP_SIZE_PIXELS, MAP_SIZE_PIXELS))
exit(0)
class narlam:
def __init__(self):
self.flag = 0
self.pause = 0
self.lidar = Lidar(LIDAR_DEVICE)
self.slam = RMHC_SLAM(LaserModel(), MAP_SIZE_PIXELS, MAP_SIZE_METERS)
#self.viz = MapVisualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS, 'SLAM')
self.trajectory = []
self.mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
self.iterator = self.lidar.iter_scans()
self.previous_distances = None
self.x = 0.0
self.y = 0.0
self.theta = 0.0
def slam_no_map(self, path_map, map_name_pgm, map_name_png):
self.flag = 0
self.pause = 0
path_map_name = path_map + '/' + map_name_pgm # map for reusing
next(self.iterator)
while True:
if self.flag == 1:
break
if self.pause == 1:
continue
items = [item for item in next(self.iterator)]
distances = [item[2] for item in items]
angles = [item[1] for item in items]
if len(distances) > MIN_SAMPLES:
f = interp1d(angles, distances, fill_value='extrapolate')
distances = list(f(np.arange(360)))
self.slam.update(distances)
self.previous_distances = distances.copy()
elif self.previous_distances is not None:
self.slam.update(self.previous_distances)
self.x, self.y, local_theta = self.slam.getpos()
local_theta = local_theta % 360
if local_theta < 0:
self.theta = 360 + local.theta
else:
self.theta = local_theta
self.slam.getmap(self.mapbytes)
# On SLAM thread termination, save map image in the map directory
# PNG format(To see/pathplannig)
image = Image.frombuffer('L', (MAP_SIZE_PIXELS, MAP_SIZE_PIXELS),
self.mapbytes, 'raw', 'L', 0, 1)
image.save(path_map + '/' + map_name_png)
# PGM format(To reuse map)
pgm_save(path_map_name, self.mapbytes,
(MAP_SIZE_PIXELS, MAP_SIZE_PIXELS))
self.lidar.stop()
self.lidar.disconnect()
def slam_yes_map(self, path_map, map_name):
self.flag = 0
self.pause = 0
path_map_name = path_map + '/' + map_name
map_bytearray, map_size = pgm_load(path_map_name)
self.slam.setmap(map_bytearray)
next(self.iterator)
while True:
if self.flag == 1:
break
if self.pause == 1:
pass
items = [item for item in next(self.iterator)]
distances = [item[2] for item in items]
angles = [item[1] for item in items]
if len(distances) > MIN_SAMPLES:
f = interp1d(angles, distances, fill_value='extrapolate')
distances = list(f(np.arange(360)))
self.slam.update(distances, should_update_map=False)
self.previous_distances = distances.copy()
elif self.previous_distances is not None:
self.slam.update(self.previous_distances,
should_update_map=False)
self.x, local_y, local_theta = self.slam.getpos()
self.y = MAP_SIZE_METERS * 1000 - local_y
local_theta = local_theta % 360
if local_theta < 0:
local_theta = 360 + local.theta
else:
local_theta = local_theta
#6/11 -> we found that the vehicle's angle was reversed on the map
self.theta = (local_theta + 180) % 360
self.slam.getmap(self.mapbytes)
self.lidar.stop()
self.lidar.disconnect()
def main():
# Bozo filter for input args
if len(argv) < 3:
print('Usage: %s <dataset> <use_odometry> <random_seed>' % argv[0])
print('Example: %s exp2 1 9999' % argv[0])
exit(1)
# Grab input args
dataset = argv[1]
use_odometry = True if int(argv[2]) else False
seed = int(argv[3]) if len(argv) > 3 else 0
# Load the data from the file
lidars, odometries = load_data('.', dataset)
# Build a robot model if we want odometry
robot = Rover() if use_odometry else None
# Create a CoreSLAM object with laser params and optional robot object
slam = RMHC_SLAM(MinesLaser(), MAP_SIZE_PIXELS, MAP_SIZE_METERS, random_seed=seed) \
if seed \
else Deterministic_SLAM(URG04(), MAP_SIZE_PIXELS, MAP_SIZE_METERS)
# Report what we're doing
nscans = len(lidars)
print('Processing %d scans with%s odometry / with%s particle filter...' % \
(nscans, \
'' if use_odometry else 'out', '' if seed else 'out'))
progbar = ProgressBar(0, nscans, 80)
# Start with an empty trajectory of positions
trajectory = []
# Start timing
start_sec = time()
# Loop over scans
for scanno in range(nscans):
if use_odometry:
# Convert odometry to velocities
velocities = robot.computeVelocities(odometries[scanno])
# Update SLAM with lidar and velocities
slam.update(lidars[scanno], velocities)
else:
# Update SLAM with lidar alone
slam.update(lidars[scanno])
# Get new position
x_mm, y_mm, theta_degrees = slam.getpos()
# Add new position to trajectory
trajectory.append((x_mm, y_mm))
# Tame impatience
progbar.updateAmount(scanno)
stdout.write('\r%s' % str(progbar))
stdout.flush()
# Report elapsed time
elapsed_sec = time() - start_sec
print('\n%d scans in %f sec = %f scans / sec' % (nscans, elapsed_sec, nscans/elapsed_sec))
# Create a byte array to receive the computed maps
mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
# Get final map
slam.getmap(mapbytes)
# Put trajectory into map as black pixels
for coords in trajectory:
x_mm, y_mm = coords
x_pix = mm2pix(x_mm)
y_pix = mm2pix(y_mm)
mapbytes[y_pix * MAP_SIZE_PIXELS + x_pix] = 0;
# Save map and trajectory as PGM file
pgm_save('%s.pgm' % dataset, mapbytes, (MAP_SIZE_PIXELS, MAP_SIZE_PIXELS))
class Slam():
def __init__(self):
# Connect to Lidar unit
self.lidar = Lidar(LIDAR_DEVICE)
# Create an RMHC SLAM object with a laser model and optional robot model
self.slam = RMHC_SLAM(LaserModel(), MAP_SIZE_PIXELS, MAP_SIZE_METERS)
# Initialize empty map
self.mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
# Set up a SLAM display
self.display = SlamShow(MAP_SIZE_PIXELS, MAP_SIZE_METERS*1000/MAP_SIZE_PIXELS, 'SLAM')
def task_update(self):
# Update SLAM with current Lidar scan, using first element of (scan, quality) pairs
(self.slam).update([pair[0] for pair in (self.lidar).getScan()])
(self.slam).getmap(self.mapbytes)
def task_getMap(self):
# Get current map bytes as grayscale
(self.slam).getmap(self.mapbytes)
def task_getPosition(self):
# Get current robot position (x, y, theta)
return self.slam.getpos()
def task_display(self):
x, y, theta = self.task_getPosition()
(self.display).setPose(x, y, theta)
(self.display).displayMap(self.mapbytes)
key = (self.display).refresh()
if key != None and (key&0x1A):
exit(0)
## Map-Interpretation
def task_exploreForward(self, distance):
x, y, theta = self.task_getPosition()
self.task_getMap()
return self.getForwardDistance(self.mapbytes, x, y, theta, distance)
def task_exploreBackward(self, distance):
x, y, theta = self.task_getPosition()
self.task_getMap()
return self.getForwardDistance(self.mapbytes, x, y, theta+180, distance)
def task_exploreLeft(self, distance):
x, y, theta = self.task_getPosition()
self.task_getMap()
return self.getLeftDistance(self.mapbytes, x, y, theta, distance)
def task_exploreRight(self, distance):
x, y, theta = self.task_getPosition()
self.task_getMap()
return self.getRightDistance(self.mapbytes, x, y, theta, distance)
def getForwardDistance(self, mapbytes, x, y, theta, minDist):
coordX = x - RANGE
coordY = y - RANGE
# convert degree to radian and rotate heading left 90 degree
angle_radL = self.getRadian(theta + LEFT)
# Find minvalue dist left side car
minDist = self.getMinDistance(mapbytes, coordX, coordY, theta, angle_radL, minDist,3)
# convert degree to radian and rotate heading right 90 degree
angle_radR = self.getRadian(theta + RIGHT)
# Find minvalue dist right side car
minDist = self.getMinDistance(mapbytes, coordX, coordY, theta, angle_radR, minDist,3)
return minDist
def getLeftDistance(self, mapbytes, x, y, theta, minDist):
coordX = x - RANGE
coordY = y - RANGE
# rotate 180 degree find back center
angle_rotate = self.getRadian(theta + ROTATE)
dX,dY = self.getCoordinate(coordX, coordY, angle_rotate,35)
# convert degree to radian and rotate heading left 90 degree
angle_radL = self.getRadian(theta+LEFT)
# Find minvalue dist left side car
minDist = self.getMinDistance(mapbytes, dX - RANGE, dY - RANGE, theta, angle_radL, minDist, 2)
return minDist
def getRightDistance(self, mapbytes, x, y, theta, minDist):
coordX = x - RANGE
coordY = y - RANGE
# rotate 180 degree find back center
angle_rotate = self.getRadian(theta + ROTATE)
dX, dY = self.getCoordinate(coordX, coordY, angle_rotate, 35)
# convert degree to radian and rotate heading right 90 degree
angle_radR = self.getRadian(theta + RIGHT)
# Find minvalue dist right side car
minDist = self.getMinDistance(mapbytes, dX - RANGE, dY - RANGE, theta, angle_radR, minDist, 2)
return minDist
def getMinDistance(self, mapbytes, coordX, coordY, theta, shiftdegree, minDist, numshift):
for shift_mm in range(1, numshift):
dx, dy = self.getCoordinate(coordX, coordY, shiftdegree, shift_mm)
dist = self.getSinglelineDistance(mapbytes, dx, dy, theta, minDist)
minDist = min(dist, minDist)
return minDist
def getSinglelineDistance(self, mapbytes, x, y, theta, maxDist):
coordX = x - RANGE
coordY = y - RANGE
angle_rad = theta * pi / 180.0
for dist_mm in range(1, maxDist + 1):
dx = coordX + cos(angle_rad) * dist_mm
dy = coordY + sin(angle_rad) * dist_mm
I = (int)(-(dy - RANGE))
J = (int)( (dx + RANGE))
index = I * RANGE + J
if mapbytes[index] < 127:
return dist_mm
return maxDist
def getCoordinate(self, coordX, coordY, angle_rad, dist_mm):
dx = coordX + cos(angle_rad) * dist_mm + (MAP_SIZE_PIXELS/2)
dy = coordY + sin(angle_rad) * dist_mm + (MAP_SIZE_PIXELS/2)
#print dx ,dy
return dx, dy
def getRadian(self,theta):
return ((theta % 360)*pi) / 180.0
def main():
# Bozo filter for input args
if len(argv) < 4:
print('Usage: %s <dataset> <output_dir> <use_odometry> [random_seed]' % argv[0])
print('Example: %s exp2 output 1 9999' % argv[0])
exit(1)
# Grab input args
dataset = argv[1]
use_odometry = True if int(argv[2]) else False
seed = int(argv[3])
output_filename = argv[4]
draw_trajectory = True if int(argv[5]) else False
print("dataset: " + dataset)
print("use_odometry: " + str(use_odometry))
print("seed: " + str(seed))
print("output_filename: " + output_filename)
print("draw_trajectory: " + str(draw_trajectory))
# Load the data from the file, ignoring timestamps
_, lidars, odometries = load_data('.', dataset)
# Build a robot model if we want odometry
robot = Robot() if use_odometry else None
# Create a CoreSLAM object with laser params and optional robot object
slam = RMHC_SLAM(Laser(), MAP_SIZE_PIXELS, MAP_SIZE_METERS, random_seed=seed) \
if seed \
else Deterministic_SLAM(Laser(), MAP_SIZE_PIXELS, MAP_SIZE_METERS)
# Report what we're doing
nscans = len(lidars)
print('Processing %d scans with%s odometry / with%s particle filter...' % \
(nscans, \
'' if use_odometry else 'out', '' if seed else 'out'))
progbar = ProgressBar(0, nscans, 80)
# Start with an empty trajectory of positions
trajectory = []
# Start timing
start_sec = time()
# Loop over scans
for scanno in range(nscans):
if use_odometry:
# Convert odometry to pose change (dxy_mm, dtheta_degrees, dt_seconds)
velocities = robot.computePoseChange(odometries[scanno])
# Update SLAM with lidar and velocities
slam.update(lidars[scanno], velocities)
else:
# Update SLAM with lidar alone
slam.update(lidars[scanno])
# Get new position
x_mm, y_mm, theta_degrees = slam.getpos()
# Add new position to trajectory
trajectory.append((x_mm, y_mm))
# Tame impatience
progbar.updateAmount(scanno)
stdout.write('\r%s' % str(progbar))
stdout.flush()
# Report elapsed time
elapsed_sec = time() - start_sec
print('\n%d scans in %f sec = %f scans / sec' % (nscans, elapsed_sec, nscans/elapsed_sec))
# Create a byte array to receive the computed maps
mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
# Get final map
slam.getmap(mapbytes)
if(draw_trajectory):
# Put trajectory into map as black pixels
for coords in trajectory:
x_mm, y_mm = coords
x_pix = mm2pix(x_mm)
y_pix = mm2pix(y_mm)
mapbytes[y_pix * MAP_SIZE_PIXELS + x_pix] = 0;
# Save map and trajectory as PGM file
pgm_save(output_filename, mapbytes, (MAP_SIZE_PIXELS, MAP_SIZE_PIXELS))
async def main():
#---------------------------------------
# Setup Lidar unit and SLAM
#---------------------------------------
# Connect to Lidar unit
lidar = nanoLidar.nanoLidar(host)
# get firmware info
print("Lidar Version:")
print(lidar.getInfo())
# 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)
#---------------------------------------
# Setup Robot unit
#---------------------------------------
# Connect to robot unit
robot = robotPosition.robotPosition(host)
robot.powerOn()
# get firmware info
print("Chassis Version:")
print(robot.getInfo())
#---------------------------------------
# main loop
#---------------------------------------
run = True
while run:
# Update SLAM with current Lidar scan, using first element of (scan, quality) pairs
slam.update([pair[0] for pair in lidar.getScan()])
# Get current robot position
x, y, theta = slam.getpos()
# 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):
run = False
# Movement control
distance = int(input("Distance [mm]: "))
angle = int(input("Turn angle [degrees]: "))
(xp, yp, theta_deg, theta_rad) = robot.setPosition(distance, angle)
if int(input("Carry on (1/0): ")) == 0:
run = False
# power off stepper motors
robot.powerOff()
# Set up a SLAM display
display = MapVisualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS, 'SLAM')
# Initialize an empty trajectory
trajectory = []
# Initialize empty map
mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
while True:
# Update SLAM with current Lidar scan, using first element of (scan, quality) pairs
slam.update([pair[0] for pair in lidar.getScan()])
# Get current robot position
x, y, theta = slam.getpos()
# Get current map bytes as grayscale
slam.getmap(mapbytes)
# Display map
display.displayMap(mapbytes)
# Display pose after converting from mm to meters
display.setPose(x / 1000., y / 1000., theta)
# Exit on window close
if not display.refresh():
exit(0)
ranges = [r * 1000 for r in d[0, 0]['Ranges'].reshape(360)]
angles = [180 * rad / 3.14159 for rad in d[0, 0]['Angles'].reshape(360)]
scans.append([ranges, angles])
for i in np.arange(0, len(scans), 1):
# Extract distances and angles from triples
distances = scans[i][0]
angles = scans[i][1]
# Update SLAM with current Lidar scan and scan angles if adequate
slam.update(distances, scan_angles_degrees=angles)
# Get current robot position
x_mm, y_mm, theta_degrees = slam.getpos()
trajectory.append((x_mm, y_mm))
def mm2pix(mm):
return int(mm / (MAP_SIZE_METERS * 1000. / MAP_SIZE_PIXELS))
slam.getmap(mapbytes)
for coords in trajectory:
x_mm, y_mm = coords
x_pix = mm2pix(x_mm)
y_pix = mm2pix(y_mm)
mapbytes[y_pix * MAP_SIZE_PIXELS + x_pix] = 0
