def create_laser(detection_angle_degrees, detection_margin=10):
return Laser(scan_size=detection_angle_degrees,
scan_rate_hz=5.5,
detection_angle_degrees=detection_angle_degrees,
distance_no_detection_mm=MAX_DIST_MM,
offset_mm=0,
detection_margin=detection_margin)
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)
clientID, proximity_sensor1, vrep.simx_opmode_streaming)
errorCode, proximity_sensor2 = vrep.simxGetObjectHandle(
clientID, 'ps2', vrep.simx_opmode_oneshot_wait)
errorCode, detectionState, detectedPoint, detectedObjectHandle, detectedSurfaceNormalVector = vrep.simxReadProximitySensor(
clientID, proximity_sensor2, vrep.simx_opmode_streaming)
#хэндлы сенсоров лидара
errorCode, vision_sensor1 = vrep.simxGetObjectHandle(
clientID, 'SICK_TiM310_sensor1', vrep.simx_opmode_oneshot_wait)
errorCode, vision_sensor2 = vrep.simxGetObjectHandle(
clientID, 'SICK_TiM310_sensor2', vrep.simx_opmode_oneshot_wait)
#объект алгоритма слама,
#параметыр лазера: 134 - количество считываемых точек, 5-частота, 270. - угол между сенсорами, 10 - максимальное расстояние обнаружения точек
#важно поставить map_quality = 1 (качество карты), иначе будет кровь из глаз
slam = RMHC_SLAM(Laser(134, 5, 270., 10),
MAP_SIZE_PIXELS,
MAP_SIZE_METERS,
map_quality=1)
viz = MapVisualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS,
'SLAM') #объект отображения построенной карты
mapbytes = bytearray(
MAP_SIZE_PIXELS *
MAP_SIZE_PIXELS) #массив байтов, который будет отображать карту
purpose = 0.3
v = 0.7
robot = pioner()
prev_pos_left = prev_pos_right = 0 #позиции левого и правого колеса
velocities = (
) #кортеж из изменения координат, изменения угла и изменения времени
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