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):
# 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')
command_handlers = {
Message.lidar_data: self.update_map,
Message.get_map: self.process_get_map_command,
Message.reset_map: self.process_reset_map_command,
}
self.message_bus.subscribe(topic = self.lidar_updates_topic_name, subscriber=self.name)
self.config.log.info("slam subscribed to %s" % self.lidar_updates_topic_name)
while True:
msg = self.message_bus.receive(self.name)
handler = command_handlers.get(msg.cmd, self.handle_invalid_command_id)
handler(msg)
return
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:
()
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 __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 __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 build(self,
laserModel,
map_size_pix,
map_size_m,
initial_position=(-1, -1)):
slam = RMHC_SLAM(LaserModel(), self.MAP_SIZE_PIXELS,
self.MAP_SIZE_METERS)
if not initial_position == (-1, -1):
slam.position = pybreezyslam.Position(initial_position[0] * 10,
initial_position[0] * 10, 0)
return 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 main():
# Bozo filter for input args
if len(argv) < 4:
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
# Allocate byte array to receive map updates
mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
# Load the data from the file
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)
# Set up a SLAM display, named by dataset
display = SlamShow(MAP_SIZE_PIXELS,
MAP_SIZE_METERS * 1000 / MAP_SIZE_PIXELS, dataset)
# Pose will be modified in our threaded code
pose = [0, 0, 0]
# Launch the data-collection / update thread
thread = Thread(target=threadfunc,
args=(robot, slam, timestamps, lidars,
odometries if use_odometry else None, mapbytes,
pose))
thread.daemon = True
thread.start()
# Loop forever,displaying current map and pose
while True:
# Display map and robot pose
display.displayMap(mapbytes)
display.setPose(*pose)
# Refresh the display, exiting gracefully if user closes it
if not display.refresh():
exit(0)
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 __init__(self):
# Connect to Lidar unit
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.left_distance_table = np.zeros(80)
self.right_distance_table = np.zeros(80)
self.thread = threading.Thread(target=self.navigate, args=())
self.thread.start()
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 __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 __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 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 __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()
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 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 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'))
# save to a png file
fig.savefig(image_filename)
return self._refresh()
# image file
image_filename = '/home/ubuntu/RCWeb/dash'
HOST, PORT = "192.168.0.18", 50007
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
s.connect((HOST, PORT))
# 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)
# We will use these to store previous scan in case current scan is inadequate
previous_distances = None
previous_angles = None
# Pin Definitons:
from flask import Flask
from flask_socketio import SocketIO, emit
from flask_cors import CORS
app = Flask(__name__)
CORS(app)
sock = SocketIO(app)
from sweeppy import Sweep as sp
from breezyslam.components import Laser
from breezyslam.algorithms import RMHC_SLAM
import json
import sys
lidar = Laser(200, 500, 360, 40000, 0, 0)
slam = RMHC_SLAM(lidar, 800, 40)
share = {'stuff': []}
lock = Lock()
def send(stuff):
sock.emit('data', stuff)
print("sent")
def scan():
with sp("/dev/tty.usbserial-DM00LDB3") as sweep:
while True:
if sweep.get_motor_ready():
break
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))
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 = (
) #кортеж из изменения координат, изменения угла и изменения времени
vrep.simxStartSimulation(clientID,
for pt in trajectory:
cv2.circle(canvas, (pt.h, pt.v), 1, (0, 0, 255), 1)
planner = MotionPlanner(trajectory, startPos, theta, canvas)
vl, vr = planner.getVelocities()
cv2.circle(canvas, (int(planner.extPos.h), int(planner.extPos.v)),
1, (255, 0, 0), 1)
cv2.circle(canvas, (destPos.h, destPos.v), 3, (255, 255, 0), 1)
cv2.line(canvas, (int(planner.extPos.h), int(planner.extPos.v)),
(startPos.h, startPos.v), (255, 0, 0), 1)
cv2.imshow('win', canvas)
cv2.waitKey(1)
pub = rospy.Publisher('velocities', Float32MultiArray, queue_size=10)
pub.publish(Float32MultiArray(data=[vl, vr]))
print('published')
def listener():
rospy.init_node('listener', anonymous=True)
rospy.Subscriber('chatter', Float32MultiArray, callback)
rospy.spin()
if __name__ == '__main__':
lidar = XVLidar()
slam = RMHC_SLAM(lidar, pixelSpan, distSpan)
listener()
from Methods import *
from LidarClass import *
from breezyslam.algorithms import RMHC_SLAM
import pandas as pd
from numpy import degrees
import matplotlib.pyplot as plt
Laser = YDlidarx2()
slam2 = RMHC_SLAM(Laser, 1000, 40)
mapbytes = bytearray(1000 * 1000)
class Flora:
pass
Flora.MSP = 1000
P = [(0, 0, 0), (1000, 0, 1)]
for i in range(2):
a = str(i)
ld = pd.read_csv('/home/pi/Flora/FloraMainCode/scans/LD0' + a + '.csv')
tmpd = ld.range * 1000
tmpa = degrees(ld.angle)
slam2.update(tmpd.tolist(), scan_angles_degrees=tmpa.tolist())
slam2.getmap(mapbytes)
Flora.mapbytes = mapbytes
#slam2.position.x_mm = 10000+1000
AA = ByteMapToNP(Flora)
plt.imshow(AA)
plt.show()
img_dict[0] = cv2.flip(cv2.imread('icons8-street-view-50.jpg'), 0)
img_dict[1] = cv2.flip(cv2.imread('icons8-staircase-50.jpg'), 0)
img_dict[2] = cv2.flip(cv2.imread('icons8-fire-extinguisher-50.jpg'), 0)
img_dict[3] = cv2.flip(cv2.imread('icons8-exit-sign-50.jpg'), 0)
img_dict[4] = cv2.flip(cv2.imread('icons8-circuit-50.jpg'), 0)
img_dict[5] = cv2.flip(cv2.imread('icons8-elevator-50.jpg'), 0)
img_dict[6] = cv2.flip(cv2.imread('icons8-test-tube-50.jpg'), 0)
img_dict[7] = cv2.flip(cv2.imread('icons8-biohazard-26.jpg'), 0)
img_dict[8] = cv2.flip(cv2.imread('icons8-radio-active-50.jpg'), 0)
img_dict[9] = cv2.flip(cv2.imread('icons8-door-opened-50.jpg'), 0)
temp = np.load('scan.npy')
print(temp)
# Connect to Lidar and initialize variables
lidar = Lidar(args.device)
slam = RMHC_SLAM(LaserModel(), args.pixelmapsize, args.metermapsize)
display = SlamShow(args.pixelmapsize,
args.metermapsize * 1000 / args.pixelmapsize, 'SLAM')
trajectory = []
mapbytes = bytearray(args.pixelmapsize * args.pixelmapsize)
iterator = lidar.iter_scans()
previous_distances = None
previous_angles = None
next(iterator)
prev_items = None
labels_dict = {}
np.save("label.npy", np.array([]))
### Primary SLAM Loop
while True:
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()
rospy.init_node("speed_controller")
speed = Twist()
r = rospy.Rate(4)
# Declare map size
MAP_SIZE_PIXELS = 50
MAP_SIZE_METERS = 10
start_x = MAP_SIZE_PIXELS / 2
start_y = MAP_SIZE_PIXELS / 2
# Create an RMHC SLAM object with a laser model and optional robot model
slam = RMHC_SLAM(TurtlebotLidar(),
MAP_SIZE_PIXELS,
MAP_SIZE_METERS,
random_seed=9999)
# 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)
x = 0.0
y = 0.0
theta = 0.0
# epsilon for testing whether a number is close to zero
_EPS = np.finfo(float).eps * 4.0
# axis sequences for Euler angles
_NEXT_AXIS = [1, 2, 0, 1]
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)
