def main(path):
print(__file__ + " start!!")
lidar = Lidar(LIDAR_DEVICE)
time.sleep(3)
try:
print('Recording measurments... Press Crl+C to stop.')
for scan in lidar.iter_scans():
#TO DO - call feature extract
#TO DO - call localizer
#TO DO - publish to network table
print(scan[0][0])
#data from each scan is in tuples: 0 = quality 1 = degrees 2 = distance
except:
print('Stoping.')
lidar.stop()
lidar.stop_motor()
lidar.disconnect()
return
def __init__(self, wheel_radius, wheel_dist, ARDUINO_HCR, LIDAR_DEVICE):
# Connect to Arduino unit
self.arduino = Serial(ARDUINO_HCR, 57600)
# Connect to Lidar unit
if LIDAR_DEVICE:
self.lidar = Lidar(LIDAR_DEVICE)
# Create an iterator to collect scan data from the RPLidar
self.iterator = self.lidar.iter_scans()
# First scan is crap, so ignore it
next(self.iterator)
self.lidar = None
self.WHEELS_DIST = wheel_dist
self.WHEELS_RAD = wheel_radius
self.x = 0
self.y = 0
self.yaw = 0
self.linear_velocity = 0
self.angular_velocity = 0
self.omegaRight = 0
self.omegaLeft = 0
self.vr = 0
self.vl = 0
self.scan = []
self.prev_time = time.time()
self.current_time = time.time()
self.dt = 0
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):
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 get_next_pts():
global lidar
global iterator
try:
return next(iterator)
except:
print('failed')
lidar.stop()
lidar.disconnect()
lidar = Lidar(LIDAR_DEVICE)
iterator = lidar.iter_scans(1000)
next(iterator)
return False
def __init__(self):
super().__init__('minimal_publisher')
# create publisher
self.publisher_ = self.create_publisher(LidarData, 'lidar_data', 10)
self.i = 0
# setup the lidar object
self.lidar = Lidar(LIDAR_DEVICE)
self.iterator = self.lidar.iter_scans(max_buf_meas = 850)
next(self.iterator)
# launch infinite loop here
while True:
self.read_lidar()
def __init__(self, station, robot_pos):
self.station = station
self.robot_pos = robot_pos
# SLAM
self.MAP_SIZE_PIXELS = 500
self.MAP_SIZE_METERS = 10
self.MIN_SAMPLES = 200
self.lidar = Lidar('/dev/ttyUSB0')
self.initial_pos_cm = (0, 0)
self.slam = SlamFactory.build(LaserModel(), self.MAP_SIZE_PIXELS,
self.MAP_SIZE_METERS, initial_pos_cm)
self.mapbytes = bytearray(self.MAP_SIZE_PIXELS * self.MAP_SIZE_PIXELS)
self.iterator = self.lidar.iter_scans()
self.previous_distances = None
self.previous_angles = None
next(self.iterator)
# scan, but on slower computers you'll get an empty map and unchanging position
# at that rate.
MIN_SAMPLES = 180
from breezyslam.algorithms import RMHC_SLAM
from breezyslam.sensors import RPLidarA1 as LaserModel
from rplidar import RPLidar as Lidar
from scipy.interpolate import interp1d
import numpy as np
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)
# 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 this to store previous scan in case current scan is inadequate
previous_distances = None
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 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_lidar(self):
self.lidar = Lidar(LIDAR_DEVICE)
# Create an iterator to collect scan data from the RPLidar
self.iterator = lidar.iter_scans()
# First scan is crap, so ignore it
next(self.iterator)
img_dict = {}
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:
def __init__(self):
super().__init__("teleop_controller")
args= sys.argv
self.name = "teleop_default_name"
self.save_index = 0
if "--name" in args:
self.name = args[args.index("--name") + 1]
print("Teleop Running: ")
print("Type(w,a,s,d,x) to move ")
print("Type 'q' to quit the node")
print("Type 'k' to activate image detection")
print("Type 'l' to de-activate image detection")
print("Type 'b' to start the Motors")
print("Type 'n' to stop the Motors")
print("Type 'f' to flip the camera")
print("Type '5' to change the map quality")
print("Type '6' to draw a line in the controller 1 logs")
print("Type '7' to take a picture now")
print("--name = ", self.name)
self.uart_publisher = self.create_publisher(String, 'uart_commands', 1000)
self.cam_control_publisher = self.create_publisher(String, 'detectnet/camera_control', 1000)
self.camera_flip_topic = self.create_publisher(String, 'video_source/flip_topic', 1000)
self.map_corner_client = self.create_client(FindMapCorner, "find_map_corner")
self.map_quality_control = self.create_publisher(String, 'slam_control', 1000)
self.log_line_printer = self.create_publisher(String, 'log_line', 5)
self.flip = True
try:
while(1):
key = self.getKey()
if key == 'l':
# stop detection
self.cam_control_publisher.publish(String(data="destroy"))
elif key == 'k':
# activate detection
self.cam_control_publisher.publish(String(data="create"))
elif key == 'b':
# start the motor
lidar = Lidar('/dev/ttyUSB0')
lidar.start_motor()
elif key == 'n':
# stop the motor
lidar = Lidar('/dev/ttyUSB0')
lidar.stop_motor()
elif key == 'f':
# flip the camera
msg = "normal" if self.flip else "flip"
self.flip = not self.flip
self.camera_flip_topic.publish(String(data=msg))
elif key == '5':
# change map quality
self.map_quality_control.publish(String(data="nimportequoi"))
elif key == '6':
self.log_line_printer.publish(String(data="pinguing"))
elif key == '7':
self.send_service()
elif key == '0':
break
else:
msg = String()
msg.data = key
self.uart_publisher.publish(msg)
raise RuntimeError("Teleoperation was aborted")
except:
print(e)
finally:
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
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 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)