def subscriber(**kwargs):
geckopy.init_node(**kwargs)
MAP_SIZE_PIXELS = 500
MAP_SIZE_METERS = 10
slam = RMHC_SLAM(LDS01_Model(), MAP_SIZE_PIXELS, MAP_SIZE_METERS)
display = SlamShow(MAP_SIZE_PIXELS,
MAP_SIZE_METERS * 1000 / MAP_SIZE_PIXELS, 'SLAM')
mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
# callback function
def f(topic, msg):
# print("recv[{}]: {}".format(topic, msg))
geckopy.loginfo(msg.timestamp)
pts = msg.scan
slam.update(pts)
# 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)
display.refresh()
geckopy.Subscriber(['scan'], f)
geckopy.spin(20) # it defaults to 100hz, this is just to slow it down
print('sub bye ...')
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)
from breezyslam.sensors import URG04LX as LaserModel
from breezylidar import URG04LX as Lidar
from pltslamshow import SlamShow
if __name__ == '__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
display = SlamShow(MAP_SIZE_PIXELS,
MAP_SIZE_METERS * 1000 / MAP_SIZE_PIXELS, 'SLAM')
# Initialize empty map
mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
while True:
# Update SLAM with current Lidar scan
slam.update(lidar.getScan())
# Get current robot position
x, y, theta = slam.getpos()
# Get current map bytes as grayscale
slam.getmap(mapbytes)
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:
# Get angles and distance from scan
try:
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()
self.scan_rate_hz = 1.57
self.detection_angle_degrees = 360
self.distance_no_detection_mm = 10000 #10m
self.detection_margin = 0
self.offset_mm = offset_mm
WINDOW_SIZE = 1200
MAP_SIZE_METERS = 10
lidar = DIYLidar(log = False)
lidar_properties = DIYLidar_Properties(lidar.getPointsPerLap())
mapbytes = bytearray(WINDOW_SIZE * WINDOW_SIZE)
slam = RMHC_SLAM(lidar_properties, WINDOW_SIZE, 35)
window = SlamShow(WINDOW_SIZE, MAP_SIZE_METERS*1000/WINDOW_SIZE, "scan")
pose = [0,0,0]
while True:
scan = lidar.waitForNextScan()
if scan != False:
slam.update(scan.getPointsDistances_mm())
x, y, theta = slam.getpos()
pose = [x /10.0, y / 10.0, theta]
slam.getmap(mapbytes)
window.displayMap(mapbytes)
window.setPose(*pose)
if not window.refresh():
