class Scanner(threading.Thread):
def __init__(self, port):
threading.Thread.__init__(self)
self.lidar = RPLidar(port)
self.lidar.stop_motor()
sleep(0.5)
self.lidar.start_motor()
self.setDaemon(True)
self.scan = [(0.0, 0.0) for i in range(360)]
def check_device(self):
info = self.lidar.get_info()
print(info)
health = self.lidar.get_health()
print(health)
return (info, health)
def get_scan(self):
return self.scan
def run(self):
self.lidar.clear_input()
while True:
for meas in self.lidar.iter_measurments():
new_scan = meas[0]
quality = meas[1]
angle = meas[2]
distance = meas[3]
if angle < 360:
self.scan[int(angle)] = (angle, distance / 1000.0)
def stop(port):
lidar = RPLidar(port)
lidar.start_motor()
sleep(0.01)
lidar.stop()
lidar.stop_motor()
lidar.disconnect()
def init():
lidar = RPLidar('/dev/ttyUSB0')
plt.plot(0, 0, 'r*')
while (1):
lidar.connect()
lidar.start_motor()
time.sleep(1)
#begins the sampling of data and places it into scan list
try:
for scan in lidar.iter_scans():
for quality, angle, distance in scan:
x_pos = (math.cos(deg_to_rad(angle)) *
distance) * 0.001 #Conversion
y_pos = (math.sin(deg_to_rad(angle)) *
distance) * 0.001 #Conversion
plt.scatter(x_pos, y_pos, color='green')
plt.pause(0.5)
except Exception as e:
pass
else:
print(e)
lidar.stop()
lidar.stop_motor()
lidar.disconnect()
break
lidar.stop()
lidar.stop_motor()
lidar.disconnect()
def run(path):
'''Main function'''
lidar = RPLidar(PORT_NAME)
lidar.disconnect()
lidar.connect()
lidar.start_motor()
lidar.stop_motor()
lidar.start_motor()
outfile = open(path, 'a')
base = time.time()
tiempo_pasado = 0
try:
print('Recording measurments... Press Crl+C to stop.')
for measurment in lidar.iter_measurments():
line = '\t'.join(str(v) for v in measurment)
tiempo_pasado = time.time() - base
line = line + '\t' + 'time\t' + str(tiempo_pasado)
outfile.write(line + '\n')
except KeyboardInterrupt:
print('Stoping.')
lidar.stop_motor()
lidar.stop()
lidar.disconnect()
outfile.close()
class Wrapper(object): # wrapper object
def __init__(self, port): # set up LiDAR connection
self.lidar = RPLidar(port)
self.lidar.stop_motor() # stop motor
def start(self): # start motor
self.lidar.start_motor()
def stop(self): # stops motor
self.lidar.stop_motor()
def output(self, amount): # samples for number of seconds
i = 0
measurements = [] # stores readings
secs = amount # 2000 samples per second
begin = time.time() # grab unix start time
for measurement in self.lidar.iter_measurments(
): # iterate through measurements
measurements.append(measurement) # add to measurement array
i = i + 1
if i == secs: # break if done sampling
end = time.time() # grab unix end times
break
print('Begin: ',
begin) # print start times, end times, and time elapsed
print('End: ', end)
print('Elapsed: ', end - begin)
self.lidar.stop_motor() # stop motor
self.lidar.stop() # stop scanning
return measurements
def run():
'''Main function'''
lidar = RPLidar(PORT_NAME)
lidar.start_motor()
time.sleep(1)
info = lidar.get_info()
print(info)
try:
print('Recording measurments... Press Crl+C to stop.')
try:
for measurment in lidar.iter_measurments():
# line = '\t'.join(str(v) for v in measurment)
# print(line + '\n')
if (measurment[2] > 0 and measurment[2] < 90):
if (measurment[3] < 1000 and measurment[3] > 100):
print("Angle: ", measurment[2] - angleoffset)
print("Distance: ", measurment[3])
steer(measurment[2] - angleoffset)
except KeyboardInterrupt:
print('Stopping.')
except KeyboardInterrupt:
print('Stopping.')
lidar.stop()
lidar.stop_motor()
c.send("motors", 0, 0, 0) # turn off wheel motors
lidar.disconnect()
def run(MOTOR_PWM):
lidar = RPLidar(PORT_NAME)
time.sleep(WAIT)
lidar.reset()
time.sleep(WAIT)
lidar._motor_speed = MOTOR_PWM
time.sleep(WAIT)
lidar.start_motor()
time.sleep(WAIT)
lidar.disconnect()
def run():
'''Main function'''
lidar = RPLidar(PORT_NAME)
lidar.start_motor()
time.sleep(1)
info = lidar.get_info()
print(info)
try:
scan(lidar)
except KeyboardInterrupt:
stop = True
print('Stopping.')
lidar.stop()
lidar.stop_motor()
lidar.disconnect()
def detect(lidar_ranges, port):
lidar = RPLidar(port)
lidar.start_motor()
sleep(1)
COUNT_LIMIT = 10
dispatcher = FilterDispatcher(lidar_ranges, COUNT_LIMIT)
try:
for _, _, angle, distance in lidar.iter_measures(scan_type='normal',
max_buf_meas=4500):
process_data(dispatcher, distance, angle)
except RPLidarException as e:
print(e)
finally:
lidar.stop()
lidar.stop_motor()
lidar.disconnect()
class Rplidar(Component):
def __init__(self, cfg):
super().__init__(inputs=[], outputs=['lidar', ], threaded=True)
self.port = cfg['lidar']
self.distance = [] # List distance, will be placed in datastorage
self.angles = [] # List angles, will be placed in datastorage
self.lidar = RPLidar(self.port)
self.on = True
self.lidar.clear_input()
def onStart(self):
"""Called right before the main loop begins"""
self.lidar.connect()
self.lidar.start_motor()
def step(self, *args):
return self.distance, self.angles,
def thread_step(self):
"""The component's behavior in its own thread"""
scans = self.lidar.iter_scans()
while self.on:
try:
for distance, angle in scans:
for item in distance: # Just pick either distance or angle since they have the same amount in list
self.distance = [item[2]] # Want to get the 3rd item which gives the distance from scans
self.angles = [item[1]] # Want to get the 2nd item which gives the angle from scans
except serial.SerialException:
print('Common exception when trying to thread and shutdown')
def onShutdown(self):
"""Shutdown"""
self.on = False
self.lidar.stop()
self.lidar.stop_motor()
self.lidar.disconnect()
def getName(self):
return 'Rplidar'
def run():
'''Main function'''
global stop
if stop == False:
lidar = RPLidar(PORT_NAME)
lidar.start_motor()
time.sleep(1)
info = lidar.get_info()
print(info)
ioloop = asyncio.get_event_loop()
tasks = [ioloop.create_task(scan(lidar)), ioloop.create_task(drive())]
try:
ioloop.run_until_complete(asyncio.wait(tasks))
except KeyboardInterrupt:
print('Stopping.')
stop = True
lidar.stop()
lidar.stop_motor()
c.send("motors", 0, 0, 0) # turn off wheel motors
lidar.disconnect()
ioloop.close()
class Lidar(Thread):
def __init__(self, port, baud, data, limit=500):
Thread.__init__(self)
self.lidar = RPLidar(port=port, baudrate=baud)
try:
print(self.lidar.get_info())
except Exception as e:
print(e)
self.lidar.eteindre()
self.lidar.reset()
self.lidar.connect()
self.lidar.start_motor()
self.data = data
self.limit = limit
def run(self):
sleep(1)
try:
for measurment in self.lidar.iter_measurments(
max_buf_meas=self.limit):
s = len(self.data[0])
if s >= self.limit:
self.data[0].pop(0)
self.data[1].pop(0)
self.data[0].append(measurment[2] * pi / 180) #agle en rad
self.data[1].append(measurment[3] / 1000) #distance en m
except Exception as error:
print(error)
def join(self, timeout=None):
self.eteindre()
Thread.join(self, timeout)
def eteindre(self):
self.lidar.stop()
self.lidar.stop_motor()
self.lidar.disconnect()
def run():
'''Main function'''
lidar = RPLidar(PORT_NAME)
lidar.start_motor()
time.sleep(1)
info = lidar.get_info()
print(info)
drive(0) # start going forward
lasttime = int(time.time() * 1000)
try:
counter = 0
print('Recording measurements... Press Crl+C to stop.')
data = 0
try:
for measurment in lidar.iter_measurments():
if ((int(time.time() * 1000) - lasttime) < 1000):
raise ValueError # do this loop at 10Hz
if (measurment[2] > 0
and measurment[2] < 90): # in angular range
if (measurment[3] < 1000
and measurment[3] > 100): # in distance range
data = data + measurment[2] # angle
counter = counter + 1 # increment counter
except ValueError:
print("this should happen ten times a second")
lasttime = int(time.time() * 1000) # reset 10Hz timer
if counter > 0: # this means we see something
average_angle = data / counter
print("Average Angle: ", average_angle - angleoffset)
drive(average_angle - angleoffset)
except KeyboardInterrupt:
print('Stopping.')
lidar.stop()
lidar.stop_motor()
c.send("motors", 0, 0, 0) # turn off wheel motors
lidar.disconnect()
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)
class LIDAR_Interface(Thread):
# These are all in Hz
_MAX_SCAN_RATE = 10
_MIN_SCAN_RATE = 0
_MAX_SCAN_PWM = 1023
_MIN_SCAN_PWM = 0
_DEFAULT_SCAN_RATE = 5.5
_GENERATOR_BUFF_SIZE = 500
_ANGULAR_TOLERANCE = 2
def __init__(self, loc: str = "/dev/ttyUSB1", sample_rate: float = 4000, scan_rate: float = 5.5, stack_depth:int = 10):
self.__lidar = RPLidar(port=loc)
super(LIDAR_Interface, self).__init__()
self.__min_parsable = 5
self.__sample_rate = sample_rate
self.__scan_rate = scan_rate
self.__samples_per_rev = LIDAR_Interface._GENERATOR_BUFF_SIZE # this may change after testing
self.__iter_scan = self.__lidar.iter_scans(self.__samples_per_rev)
self.__stack = Stack(stack_depth)
self.__current_scan = []
self.__prev_scan = []
self._max_distance = 5000
self._min_distance = 0
self.__running = False
atexit.register(self.exit_func)
# control functions
def stop_thread(self):
self.__running = False
def exit_func(self):
self.stop_thread()
self.stop_motor()
self.stop_sensor()
self.__lidar.disconnect()
def stop_sensor(self):
self.__lidar.stop()
def stop_motor(self):
self.__lidar.stop_motor()
def reset_sensor(self):
self.__lidar.reset()
self.__lidar.clear_input()
def start_motor(self):
self.__lidar.start_motor()
@property
def running(self):
return self.__running
# properties
@property
def max_distance(self):
return self._max_distance
@max_distance.setter
def max_distance(self, distance: int):
if distance > self._min_distance:
if distance < 5000:
self._max_distance = distance
@property
def min_distance(self):
return self._min_distance
@min_distance.setter
def min_distance(self, distance: int):
if distance >= 0:
if distance < self._max_distance:
self._min_distance = distance
@property
def sensor_health(self):
return self.__lidar.get_health()
@property
def sensor_info(self):
return self.__lidar.get_info()
@property
def sample_rate(self):
return self.__sample_rate
@property
def scan_rate(self):
return self.__scan_rate
@scan_rate.setter
def scan_rate(self, value: float):
if LIDAR_Interface._MAX_SCAN_RATE >= value >= LIDAR_Interface._MIN_SCAN_RATE:
self.__scan_rate = value
self.__sample_rate = LIDAR_Interface._GENERATOR_BUFF_SIZE * self.__scan_rate
self._set_scan_rate()
@property
def pop_recent_scan(self) -> list:
if self.__stack.length > 0:
return self.__stack.pop()
return list()
@property
def recent_scan(self) -> list:
if self.__stack.length > 0:
return self.__stack.top
return list()
@property
def stack(self) -> Stack:
return self.__stack
# conversion function
@staticmethod
def _map(x: float, from_min: float, from_max: float, to_min: float, to_max: float) -> float:
return (x - from_min) * (to_max - to_min) / ((from_max - from_min) + to_min)
# Motor control functions
def _set_scan_rate(self):
self.__lidar.set_pwm(self._map(self.__scan_rate, LIDAR_Interface._MIN_SCAN_RATE, LIDAR_Interface._MAX_SCAN_RATE,
LIDAR_Interface._MIN_SCAN_PWM, LIDAR_Interface._MAX_SCAN_PWM))
# thread function
def start(self) -> None:
super(LIDAR_Interface, self).start()
self.__running = True
def run(self) -> None:
while self.__running:
# iter must produce a full rotation (360 points) before we can use it
#_scan = self.__lidar.iter_scans(min_len=self.__samples_per_rev)
if self.__iter_scan.__sizeof__() > 0:
_scan = next(self.__iter_scan)
for scan in _scan:
current = Ray(scan[2], scan[1], scan[0])
if current.radius > self._min_distance:
if current.radius < self._max_distance:
self.__current_scan.append(current)
# if the current scan has the total points for a rotation we can consume it and reset the current scan
if self.__current_scan.__len__() >= 360:
self.__stack.push(self.__current_scan.copy())
self.__current_scan.clear()
self.__lidar.stop()
self.__lidar.stop_motor()
self.__lidar.disconnect()
)**2 #Cam - determines how closely the lines follow each other- Simulation Purposes Only
Rsim = np.diag(
[1.0, np.deg2rad(30.0)]
)**2 # Cam - Changes the correlation between dead reckoning and actual posiitoning
DT = 0.1 # time tick [s] #Cam - how many times it updates over the course of the simulation, but also changes the simulation time by a predictable about (multiply DT by 9 = divide sim time by 9)
SIM_TIME = 50.0 # simulation time [s] # Cam - how many "seconds", partially relative to DT (see comment above), in the simulation
MAX_RANGE = 20.0 # maximum observation range # Cam - maximum domain and range of the simulation coordinate plane
# Particle filter parameter
NP = 100 # Number of Particle # Cam - exactly what they said
NTh = NP / 2.0 # Number of particle for re-sampling # Cam - how many points seem reasonable to the program
show_animation = True #Cam - determines if the animation will be shown
lidar.start_motor()
#lidar.connect()
def scan(path):
'''Main function'''
for measurment in lidar.iter_measurments():
print(measurment[2])
def calc_input():
print("running calc")
#print(lidar.measurment[0])
v = 1.0 # [m/s] #Cam - velocity of the simulated robot
yawrate = 0.1 # [rad/s] #Cam- rotational rate of the robot
u = np.array([[
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)
