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)
def find_exact_vanes(self, lower_angle_L, lower_angle_R, upper_angle_L, upper_angle_R, lower_distance_L, lower_distance_R,
upper_distance_L, upper_distance_R):
mylidar = RPLidar("COM3", baudrate=115200)
mylidar_scan = []
total_average_left_vane = []
total_average_right_vane = []
total_average_angle_left_vane = []
total_average_angle_right_vane = []
for y in range(0, 20):
for i, scan in enumerate(mylidar.iter_scans(scan_type='normal',
max_buf_meas=60000)): # scan_type='normal', max_buf_meas=60000
mylidar_scan.append(scan)
if i > 10:
break
for i in range(len(mylidar_scan)): # aantal rondes
left_vane = []
right_vane = []
for j in range(len(mylidar_scan[i])): # aantal metingen in het rondje
my_list = mylidar_scan[i][j]
if lower_angle_L < my_list[1] < upper_angle_L and lower_distance_L < my_list[2] < upper_distance_L:
left_vane.append(my_list)
elif lower_angle_R < my_list[1] < upper_angle_R and lower_distance_R < my_list[2] < upper_distance_R:
right_vane.append(my_list)
# print("arr_avg: ", arr_avg)
if left_vane:
left_vane = np.array(left_vane)
average_left_vane = np.mean(left_vane[:, 2])
average_angle_left_vane = np.mean(left_vane[:, 1])
total_average_left_vane.append(average_left_vane)
total_average_angle_left_vane.append(average_angle_left_vane)
print("Average left", average_left_vane)
if right_vane:
right_vane = np.array(right_vane)
average_right_vane = np.mean(right_vane[:, 2])
average_angle_right_vane = np.mean(right_vane[:, 1])
total_average_right_vane.append(average_right_vane)
total_average_angle_right_vane.append(average_angle_right_vane)
print("Average right", average_right_vane)
mylidar.clean_input()
grand_total_left = np.mean(total_average_left_vane)
grand_total_right = np.mean(total_average_right_vane)
grand_total_left_angle = np.mean(total_average_angle_left_vane)
grand_total_right_angle = np.mean(total_average_angle_right_vane)
print("totaal links:", grand_total_left)
print("totaal rechts:", grand_total_right)
print("totaal hoek links:", grand_total_left_angle)
print("totaal hoek rechts:", grand_total_right_angle)
mylidar.stop()
mylidar.stop_motor()
mylidar.disconnect()
return grand_total_left, grand_total_right, grand_total_left_angle, grand_total_right_angle
class hcr():
def __init__(self, ARDUINO_PORT='/dev/ttyACM0', LIDAR_PORT='/dev/ttyUSB0'):
self.connect = self.openconnect(ARDUINO_PORT, ARDUINO_SPEED)
# Connect to Lidar unit
self.lidar = RPLidar(LIDAR_PORT)
# Create an iterator to collect scan data from the RPLidar
self.iterator = self.lidar.iter_scans()
def check_connect(self, connect):
c = connect.read(1).decode()
if c != 'c':
print("false read")
def openconnect(self, port, speed):
connect = serial.Serial(port, speed)
time.sleep(1)
while not connect.is_open:
self.openconnect(port, speed)
is_connected = False
while not is_connected:
print("Waiting for arduino...")
connect.write(start_connect.encode())
connect_flag = connect.read(1).decode()
self.check_connect(connect)
if not connect_flag:
time.sleep(0.1)
continue
if connect_flag == 'r':
is_connected = True
print('Connected!')
return connect
def send(self, lvel, avel):
send_data = set_command + str(round(lvel,2)) + ' ' + str(round(avel,2)) + "\n"
self.connect.write(send_data.encode())
self.check_connect(self.connect)
def lidar_stop(self):
self.lidar.stop()
self.lidar.stop_motor()
self.lidar.disconnect()
def arduino_stop(self):
self.setMotors(0,0)
self.connect.close()
def stop(self):
self.lidar_stop()
self.arduino_stop()
def getMotors(self):
self.connect.write(print_command.encode())
data = self.connect.read(12).decode()
#print(data)
self.check_connect(self.connect)
data = data.split(';')
right = float(data[0])
left = float(data[1])
return right, left
def setMotors(self, rightVelocity, leftVelocity):
self.send(rightVelocity, leftVelocity)
#print('2')
#self.check_connect(self.connect)
def getScan(self):
#get laser dara
# Extract (quality, angle, distance) triples from current scan
scan = [[item[1], item[2]] for item in next(self.iterator)]
return scan
def LidarAlign360(path):
'''Main function'''
loop = False
Inches = None
lidarDegrees = 0
reset = 10
baseMeasurement= 3000
ultimateMeasurement = 10
PORT_NAME = '/dev/ttyUSB0'
lidar = RPLidar(PORT_NAME)
outfile = open(path, 'w')
ZeroBlocked = False
NinetyBlocked = False
OneEightyBlocked = False
TwoSeventyBlocked = False
try:
print('Recording measures... Press Crl+C to stop.')
for measurment in lidar.iter_measures():
distance = measurment[3]
degrees = measurment[2]
if (degrees <= 185 and degrees >= 175):
loop = True
if (degrees >= 0 or degrees <= 360) and distance != 0:
#print '[ultimateMeasurement %f]' % (ultimateMeasurement)
#print '[degrees %f]' % (degrees)
#print '[distance %f]' % (distance)
#if distance < baseMeasurement:
#ultimateMeasurement = distance
if(degrees >= 358 and degrees <= 2):
if distance >= ultimateMeasurement:
ultimateMeasurement = distance
lidarDegrees = degrees
if loop == True:
loop = False
Inches = ultimateMeasurement/25.4
print "[inches %f]", (Inches)
ultimateMeasurement = baseMeasurement
if (degrees >= 358 or degrees <= 2) and Inches <= 10:
ZeroBlocked = True;
elif (degrees >= 88 and degrees <= 92) and Inches <= 10:
NinetyBlocked = True;
elif (degrees >= 178 and degrees <= 182) and Inches <= 10:
OneEightyBlocked = True
elif (degrees >= 268 and degrees <= 272) and Inches < = 10:
TwoSeventyBlocked = True
else:
ZeroBlocked = False
NinetyBlocked = False
OneEightyBlocked = False
TwoSeventyBlocked = False
if ZeroBlocked == False and NinetyBlocked == True and TwoSeventyBlocked == True:
sd.putDouble("MazeSpeed", 0.5)
if OneEightyBlocked = False and ZeroBlocked == True and NinetyBlocked = True and TwoSeventyBlocked = True:
sd.putDouble("MazeSpeed", -0.5)
if
sd.putNumber("left", left)
sd.putNumber("right", right)
print ("lidarDegrees", lidarDegrees)
print ("right", right)
print ("left", left)
#print'[robotSpeed = %f]' % (robotSpeed)
#sd.putNumber('ultimateMeasurement', ultimateMeasurement)
#if loop == True:
#ultimateMeasurement = baseMeasurement
except KeyboardInterrupt:
print('Stopping.')
except:
traceback.print_exc()
lidar.stop_motor()
lidar.stop()
lidar.disconnect()
outfile.close()
class RPLiDAR:
def __init__(self, sectors):
# self.lidar = RPLidar("\\\\.\\com4") # Check to see if this runs on mac
self.lidar = RPLidar("/dev/ttyUSB0")
# laptop. If not make change. May be the /dev/ thing
self.sectors = sectors
self.sector_space = {}
self.prev_sector_space = {}
time.sleep(1)
info = self.lidar.get_info()
health = self.lidar.get_health()
print(info, health, sep='\n')
self.file = open('lidar-data.csv', 'w')
self.writer = csv.writer(self.file)
def scan_area(self, limit=100):
for i, scan in enumerate(self.lidar.iter_scans()):
if i > limit:
break
print('%d: Got %d measurements' % (i, len(scan)))
sector_space = np.zeros(6)
warning_flag = False
for j in scan:
k = math.floor(j[1] / 60)
if j[2] < 1000:
# print("Warning: object at %f degrees is too close"
# % (j[1]))
sector_space[k] += -math.inf # Set to say space is unsafe
warning_flag = True
else:
sector_space[k] += j[2] # adding distance to sector array
if warning_flag:
print(sector_space) # outputs six values
print("Object(s) are too close...")
free_space = max(sector_space)
if free_space < 1000:
print("There is nowhere safe to venture, immediate landing"
" to avoid damage...")
break
evacuation_direction = \
(sector_space.index(free_space)+1) * 60 - 30
print("Evacuate in the direction of %d degrees" %
evacuation_direction) # This is the safest spot
def area_report(self, limit=100):
for i, scans in enumerate(self.lidar.iter_scans()):
if i > limit:
# self.stop()
break
self.sector_space = {}
divisor = 360 // self.sectors
for scan in scans:
section = math.floor(scan[1] / divisor)
try:
self.sector_space[section] = np.append(
self.sector_space[section], scan[2])
except KeyError:
self.sector_space[section] = np.array(scan[2])
evaluation_space = self._evaluate_spcae()
# print('evaluate space at ', time.time(), evaluation_space,
# file=self.file)
# self.file.write('evaluate space \n' + str(evaluation_space))
direction = self._get_direction(evaluation_space)
if direction == -1:
print('There are no safe regions!')
print('Go to region %d' % direction)
self._write_file(evaluation_space, direction)
def _evaluate_spcae(self):
evaluation = []
for i in range(self.sectors):
try:
section = self.sector_space[i]
evaluation.append((i, np.min(section), np.max(section),
np.average(section), section))
except KeyError:
evaluation.append((None, i, None, None, None))
return evaluation
def _get_direction(self, evaluation_space):
current_section = -1
previous_min = 1
for value in evaluation_space:
# print(value)
section, min, max, average, sector = value
try:
if min > previous_min:
current_section = section
previous_min = min
except TypeError:
pass
return current_section
def _write_file(self, evaluation_space, direction):
self.writer.writerow('sector number: %d' % direction)
for values in evaluation_space:
self.writer.writerow((datetime.datetime.time(
datetime.datetime.now()).strftime('%H:%M:%S'), values))
def stop(self):
self.lidar.stop()
self.lidar.stop_motor()
self.lidar.disconnect()
self.file.close()
class RPLidar(object):
'''
https://github.com/SkoltechRobotics/rplidar
'''
def __init__(self, port='/dev/ttyUSB0'):
from rplidar import RPLidar
self.port = port
self.distances1 = 0
self.angles1 = 0
self.distances2 = 0
self.angles2 = 0
self.distances3 = 0
self.angles3 = 0
self.lidar = RPLidar(self.port)
self.lidar.clear_input()
time.sleep(1)
self.on = True
#print(self.lidar.get_info())
#print(self.lidar.get_health())
def update(self):
scans = self.lidar.iter_scans(550)
while self.on:
try:
d1 = 0
d1_num = 0
a1 = 0
a1_num = 0
d2 = 0
d2_num = 0
a2 = 0
a2_num = 0
d3 = 0
d3_num = 0
a3 = 0
a3_num = 0
for scan in scans:
for index in range(len(scan)):
if scan[index][1] >= 90 and scan[index][1] < 150:
d1 += scan[index][2]
d1_num = d1_num + 1
a1 += scan[index][1]
a1_num = d1_num + 1
elif scan[index][1] >= 150 and scan[index][1] < 210:
d2 += scan[index][2]
d2_num = d1_num + 1
a2 += scan[index][1]
a2_num = d1_num + 1
elif scan[index][1] >= 210 and scan[index][1] < 270:
d3 += scan[index][2]
d3_num = d1_num + 1
a3 += scan[index][1]
a3_num = d1_num + 1
elif (int(scan[index][1])) == 359 and d1_num != 0:
self.distances1 = d1 / d1_num
self.angles1 = a1 / a1_num
self.distances2 = d2 / d2_num
self.angles2 = a2 / a2_num
self.distances3 = d3 / d3_num
self.angles3 = a3 / a3_num
d1 = 0
d1_num = 0
a1 = 0
a1_num = 0
d2 = 0
d2_num = 0
a2 = 0
a2_num = 0
d3 = 0
d3_num = 0
a3 = 0
a3_num = 0
'''
print ("angles: ",self.angles2)
print ("distances: ",self.distances2)
'''
'''
self.distances = [item[2] for item in scan]
self.angles = [item[1] for item in scan]
'''
except serial.serialutil.SerialException:
print(
'serial.serialutil.SerialException from Lidar. common when shutting down.'
)
def run_threaded(self):
return self.distances1, self.angles1, self.distances2, self.angles2, self.distances3, self.angles3
def run(self):
return self.distances1, self.angles1
def shutdown(self):
self.on = False
time.sleep(2)
self.lidar.stop()
self.lidar.stop_motor()
self.lidar.disconnect()
class GluttonousSnake:
def __init__(self, state = State.WAIT):
self.state = state
self.running = True
self.car_srl = Car_srlapp()
self.lidar = RPLidar('/dev/ttyUSB0')
# 状态控制
def _state_control(self):
while self.running and self._get_state() is not State.STOP:
dist, _ = self._get_nearest_target()
# 加锁避免状态数据同步冲突
threadLock.acquire()
if self._get_state() is State.WAIT and dist < Wait2HeadThreshold:
self.state = State.HEAD
elif self._get_state() is State.HEAD and dist < Head2FollowThreshold:
self.state = State.FOLLOW
elif self._get_state() is State.WAIT and dist < Follow2StopThreshold:
self.state = State.STOP
threadLock.release()
sleep(StateInspectInterval)
def _wait_main(self):
while self.running and self._get_state() is State.WAIT:
pass
def _head_main(self):
while self.running and self._get_state() is State.HEAD:
dest = self._get_nearest_target()
# 判断是否存在目标,存在则前往，不存在则停止
# 在state_control外进行了状态切换，代码结构上有缺陷，但性能上更好
if dest:
self._go_somewhere(dest)
else:
# 加锁避免状态数据同步冲突
threadLock.acquire()
self.state = State.STOP
threadLock.release()
sleep(HeadPathCorrectionInterval)
def _follow_main(self):
while self.running and self._get_state() is State.FOLLOW:
dest = self._get_nearest_target()
# 考虑前车停止的碰撞问题，对dest进行修正
dest = self._follow_dest_correction(dest)
self._go_somewhere(dest)
sleep(FollowPathCorrectionInterval)
def _stop_main(self):
while self.running and self._get_state() is State.STOP:
self.car_srl.control(MgsType.CONTROL_STOP)
self.running = False
def main(self):
state_control = threading.Thread(target=self._state_control, args=())
wait_main = threading.Thread(target=self._wait_main(), args=())
head_main = threading.Thread(target=self._head_main(), args=())
follow_main = threading.Thread(target=self._follow_main(), args=())
stop_main = threading.Thread(target=self._stop_main(), args=())
threads = {State.HEAD: head_main,
State.FOLLOW: follow_main,
State.STOP: stop_main}
cur_state = self._get_state()
# 如果当前状态为wait，则将wait_main加入threads，便于后续遍历join
if cur_state is State.WAIT:
threads[cur_state] = wait_main
# pre_state用于判断状态切换，以开启新线程
pre_state = cur_state
state_control.start()
threads[pre_state].start()
while self.running:
if pre_state is not self._get_state():
threads[self._get_state()].start()
pre_state = self._get_state()
for thread in threads.values():
thread.join()
state_control.join()
self.lidar.stop()
self.lidar.stop_motor()
self.lidar.disconnect()
def _get_state(self):
return self.state
# 雷达数据获取
def _get_radar_signal(self):
scan = next(self.lidar.iter_scans(300, 200))
radar_signal = {}
for i in scan:
radar_signal[i[1]] = i[2]
return radar_signal
# 控制小车朝向dest运动
def _go_somewhere(self, dest):
_, yaw, _ = self.car_srl.request(MgsType.REQUEST_POSE)
# yaw的顺逆时针朝向未知，故此处为+或-待定
yaw_zero = yaw - dest[1]
self.car_srl.control(MgsType.CONTROL_TURN, TurnSpeed)
while abs(yaw_zero - yaw) > AngleDeviationThreshold:
sleep(AngleCorrectionInterval)
_, yaw, _ = self.car_srl.request(MgsType.REQUEST_POSE)
self.car_srl.control(MgsType.CONTROL_STOP)
self.car_srl.control(MgsType.CONTROL_MOVE, MoveSpeed)
# 获取数据梯度信息
def _get_grads(self, sorted_signal):
grads = []
for index in range(1, len(sorted_signal)):
delta = sorted_signal[index - 1][0] - sorted_signal[index][0]
theta = sorted_signal[index - 1][0]
grad = (sorted_signal[index - 1][1] - sorted_signal[index][1]) / delta
grads.append((theta, grad))
return grads
# 根据梯度信息获取所有目标的角度（左右边界角度的平均值）及距离（目前使用的左边界距离）
def _get_targets(self, grads, radar_signal):
targets = []
temp_boundary = 0
for i in grads:
if i[1] < leftGradThreshold and temp_boundary == 0:
temp_boundary = i[0]
elif i[1] > rightGradThreshold and temp_boundary != 0:
targets.append((radar_signal[i[0]], (temp_boundary + i[0])/2))
temp_boundary = 0
return targets
# 返回范围内最近目标的位置信息
def _get_nearest_target(self):
# TODO 跨越360~0边界的目标检测还有BUG存在
radar_signal = self._get_radar_signal()
# 筛选所需范围的雷达数据
radar_signal_ranged = {}
for i in radar_signal:
if scanned_range[self._get_state()][0] <= i <= scanned_range[self._get_state()][1]:
radar_signal_ranged[i] = radar_signal[i]
# 对筛选后的数据进行排序
sorted_signal = sorted(radar_signal_ranged.items(), key=operator.itemgetter(0))
grads = self._get_grads(sorted_signal)
targets = self._get_targets(grads, radar_signal)
# 考虑搜寻不到目标的情况
if targets:
target = min(targets)
else:
target = ()
return target
# TODO 对跟随目标修正，以避免碰撞
def _follow_dest_correction(self, dest):
return dest
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()
class AnimatedScatter(object):
"""An animated scatter plot using matplotlib.animations.FuncAnimation. WhichLidar -> 0 = rplidar, 1 = NONE, 2 = hokuyo
MaxRange, AxisRange - ranges only for visualization
lidarMinThresh, lidarMaxThresh - distance thresholds for what data is taken from the lidar reading
whichCenter - only for demonstration purposes - if 0 then lidar is center of coordinate system, if 1 then the lidar's position is calculated from all it's readings to a arbitrary 0 based coordinate system
"""
def __init__(self,portName = '/dev/ttyACM0', maxRange = 30000, axisRange = 10000, whichLidar = 2, lidarMinThresh = 500, lidarMaxThresh = 5000, whichCenter = 0):
#===============================================================#
#=================== Initialization Block ======================#
#===============================================================#
self.maxRange = maxRange
self.axisRange = axisRange
self.whichLidar = whichLidar
self.portName = portName
self.lidarMinThresh = lidarMinThresh
self.lidarMaxThresh = lidarMaxThresh
self.whichCenter = whichCenter
# global variables
self.anglePCA = 0 # angle of the blade points, calculated using PCA
self.PCACalculated = False # helper bool for determining if blade angle calculated
self.environmentPoints = [] # detected points from blade
self.calculateEllipse = False # is the ellipse calculated
self.ellipseAlgStart = False # is the elliptical algorithm running
self.serial_IMU = serial.Serial("COM10",57600) # IMU + arduino serial, if no IMU is present please comment out
self.arduinoInitial = np.zeros(8)#if no IMU is present please comment out
self.lidarRotAngle = 0 # lidar rotation angle , if no IMU is present please comment out
self.armedAngle = 0 # lidar angle when the blade angle is calculated, used as an initial angle, if no IMU is present please comment out
# initialize lidar - in case of the rpLidar an initial health check is required to be sure that the proper data is sent
if self.whichLidar is 0:
self.lidar = RPLidar(self.portName)
print("Starting Lidar...")
## Start Lidar and get's info
while True:
try:
info = self.lidar.get_health()
break
except RPLidarException:
print("Lidar error retry")
finally:
self.lidar.stop()
print(info)
self.iterator = self.lidar.iter_scans(1000,5,100,6000)
elif self.whichLidar is 1:
pass
# The sweep lidar is removed as it is not currently used
elif self.whichLidar is 2:
self.lidar = lx.connect(self.portName)
lx.startLaser(self.lidar)
#===============================================================#
#========================== BLOCK END ==========================#
#===============================================================#
#===============================================================#
#================== Setup figures and events ===================#
#===============================================================#
# Setup the figure and axes...
self.fig, self.ax = plt.subplots()
self.fig.canvas.mpl_connect('close_event', self.handle_close) # event for clicking the X
self.onClickEv = self.fig.canvas.mpl_connect('button_press_event', self.on_click) # event for clicking the mouse button
# Then setup FuncAnimation. - self.update is the update function, while self.setup_plot is the initialization of figures
self.ani = animation.FuncAnimation(self.fig, self.update, interval=1./40,
init_func=self.setup_plot, blit=True)
#===============================================================#
#========================== BLOCK END ==========================#
#===============================================================#
# Handler function for closing the figure, each lidar has different exit strategies
def handle_close(self,evt):
print('Closed Figure!')
self.fig.canvas.mpl_disconnect(self.onClickEv)
self.serial_IMU.close() # disconnect IMU serial, if no IMU is present please comment out
if self.whichLidar is 0:
self.lidar.stop()
self.lidar.stop_motor()
self.lidar.disconnect()
elif self.whichLidar is 1:
pass
elif self.whichLidar is 2:
self.lidar.close()
# Handler function for mouse click on the canvas
def on_click(self, evt):
# if the blade points orientation is calculated and the elliptical alg is not started - start it now
if self.PCACalculated == True and self.ellipseAlgStart == False:
self.ellipseAlgStart = True
# if the blade points orientation is not calculated - do it
if self.PCACalculated == False:
self.anglePCA = calculateAnglesPCA(self.environmentPoints)
self.PCACalculated = True
print(self.anglePCA)
print("Clicked")
# Setup function for plotting
def setup_plot(self):
"""Setup static markings."""
# Setup axis limits
self.ax.axis([-self.axisRange, self.axisRange, -self.axisRange, self.axisRange])
# Create updating scatter plot and variable for later use
self.scat = self.ax.scatter([], [], c='b', s=1, animated=True, alpha=0.5)
self.scat_lidar = self.ax.scatter([], [], c='r', s=30, animated=True, alpha=0.5)
self.scat_ellipse = self.ax.scatter([], [], c='g', s=20, animated=True, alpha=0.5)
# For FuncAnimation's sake, we need to return the artist we'll be using
# Note that it expects a sequence of artists, thus the trailing comma.
return self.scat,self.scat_lidar,self.scat_ellipse,
def update(self, i):
"""Update the scatter plot."""
# Update function runs and gets data from the Lidar
if self.whichLidar is 0:
scan = next(self.iterator)
angleDistance = np.array(scan)
elif self.whichLidar is 1:
pass
elif self.whichLidar is 2:
# 0 and 1080 is the degrees that the Hokuyo gets readings from - 0 to 180 degrees, as it checks in 6 increments between a degree
# the last value of 1 signifies if the data will be clustered - if it's 1 data is not averaged by captures
angleDistance, status = lx.getLatestScanSteps(self.lidar, 0, 1080,1)
# Remove data that is farther or closer than the thresholds
angleDistance = angleDistance[np.logical_and(angleDistance[:,1] > self.lidarMinThresh, angleDistance[:,1] < self.lidarMaxThresh)]
# Used only for testing/visualization purposes - when 0 only the environment points are printed and the lidar is always at 0,0
if self.whichCenter == 0:
environmentPoints = circle2cart_points([0,0],angleDistance, 0)
lidarPoint = [0,0]
# Start real algorithm
elif self.whichCenter == 1:
# Get orientation data from IMU, if no IMU present comment out the next three lines
arduinoOutput = getDataFromIMU(self.serial_IMU,self.arduinoInitial)
self.arduinoInitial = arduinoOutput
self.lidarRotAngle = arduinoOutput[4]
# if the elliptical algorithm is started compensate for the lidar's rotation
if self.ellipseAlgStart is True:
compensateYaw = self.lidarRotAngle - self.armedAngle # current rotation angles - the armed angle
angleDistance[:,0] = angleDistance[:,0] + compensateYaw
# Calculate mean angle and mean distance
meanAngle,meanDist = calculateMeans(angleDistance)
# go from polar to carthesian system - position the lidar at a 0,0 coordinate system, then reproject blade points from the lidar's position
lidarPoint = circle2cart_drone([0,0],meanAngle, meanDist)
environmentPoints = circle2cart_points(lidarPoint,angleDistance, 0)
self.environmentPoints = environmentPoints
# is the blade orientation calculated
if self.PCACalculated is True:
if self.calculateEllipse is False:
# Calculate ellipse - first get the major diameter of the ellipse
minDist,maxDist,minDist_points,maxDist_points = calculateMinMax(self.environmentPoints)
# create ellipse using the major diameter, and major diameter/6 as minor diameter, the angle of rotation and a 0,0 center
ellipseRadAngle, self.ellipseBladePoints = testEllipse(int(maxDist/6), int(maxDist), self.anglePCA, [0,0])
self.calculateEllipse = True
# save the armed angle of lidar orientation
self.armedAngle = self.lidarRotAngle
if self.ellipseAlgStart is True:
# get the average detected point position
averagePointPos=[sum(environmentPoints[:,0])/len(environmentPoints[:,0]),sum(environmentPoints[:,1])/len(environmentPoints[:,1])]
pCenter=np.array((0,0))
pAvg = np.array((averagePointPos[0],averagePointPos[1]))
# calculate the distance between the ellipse center and the point
distAveragePointToZero = np.linalg.norm(pCenter-pAvg)
# find the intersection between the ellipse the center of the ellipse and the lidar position
intersectPointOnCurve = intersectionLineCurve([0,0], lidarPoint, self.ellipseBladePoints)
# calculate correction distance - ellipse radius
correctionDist = math.sqrt(intersectPointOnCurve[0]**2 + intersectPointOnCurve[1]**2)
# calculate new center for going from polar to carthesian using the correction distance and the dist of the average point to zero
newCenterPos = circle2cart_drone([0,0],meanAngle, correctionDist -distAveragePointToZero)
lidarPoint = circle2cart_drone(newCenterPos,meanAngle, meanDist)
environmentPoints = circle2cart_points(lidarPoint,angleDistance, 0)
# visualize ellipse
self.scat_ellipse.set_offsets(self.ellipseBladePoints)
# Visualize environment and lidar points
self.scat.set_offsets(environmentPoints)
self.scat_lidar.set_offsets(lidarPoint)
# We need to return the updated artist for FuncAnimation to draw..
# Note that it expects a sequence of artists, thus the trailing comma.
return self.scat,self.scat_lidar,self.scat_ellipse,
def show(self):
plt.show()
class PromobotAPI:
port = None
server_flag = False
last_key = -1
last_frame = np.array([[10, 10], [10, 10]], dtype=np.uint8)
quality = 50
manual_mode = 0
manual_video = 1
manual_speed = 150
manual_angle_yaw = 70
manual_angle_pitch = 70
frame = []
mouse_x = -1
mouse_y = -1
joy_x = 0
joy_y = 0
small_frame = 0
motor_left = 0
motor_right = 0
encode_param = 0
flag_serial = False
flag_camera = False
t = 0
measurements = []
process = None
lidar_port = "COM5"
lidar = None
isLidarWorking = False
k = 0
fps_timer = 0
def __init__(self, flag_serial=True, flag_camera=True):
# print("\x1b[42m" + "Start script" + "\x1b[0m")
self.flag_camera = flag_camera
self.flag_serial = flag_serial
print("Start script")
atexit.register(self.cleanup)
# print("open robot port")
if flag_serial:
import serial
self.port = serial.Serial('COM3', 2000000, timeout=2)
time.sleep(1.5)
print("Arduino connected")
# vsc.VideoClient.inst().subscribe("ipc")
# vsc.VideoClient.inst().subscribe("tcp://127.0.0.1")
# vsc.VideoClient.inst().subscribe("udp://127.0.0.1")
# while True:
# frame = vsc.VideoClient.inst().get_frame()
# if frame.size != 4:
# break
if flag_camera:
self.cap = cv2.VideoCapture()
self.cap.open(0)
r, self.frame = self.cap.read()
self.time_frame = time.time()
# self.cap.set(cv2.CAP_PROP_FPS, 30)
# self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, 640)
# self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 480)
# self.cap.open(0)
# if camera_high_res:
# self.set_camera_high_res()
# if flag_video:
# self.context = zmq.Context(1)
# self.socket = self.context.socket(zmq.REP)
#
# self.socket.bind("tcp://*:5555")
# # print("start video server")
# self.server_flag = True
# self.my_thread_video = threading.Thread(target=self.send_frame)
# self.my_thread_video.daemon = True
#
# self.my_thread_video.start()
# if flag_keyboard:
# # self.context2 = zmq.Context(1)
# # self.socket2 = self.context2.socket(zmq.REP)
# #
# # self.socket2.bind("tcp://*:5559")
# # # print("start video server")
# # self.server_keyboard = True
# self.my_thread = threading.Thread(target=self.receive_key)
# self.my_thread.daemon = True
# self.my_thread.start()
# серву выставляем в нуль
# if self.flag_serial:
# self.servo(0)
# очищаем буфер кнопки( если была нажата, то сбрасываем)
# self.button()
# выключаем все светодиоды
if self.flag_serial:
self.servo_yaw(70)
self.servo_pitch(70)
self.lidar_start()
self.lidar_stop()
self.stop_frames = False
if flag_camera:
self.my_thread_f = threading.Thread(target=self.work_f)
self.my_thread_f.daemon = True
self.my_thread_f.start()
self.manual_video = 1
pass
def end_work(self):
# self.cap.release()
self.stop_frames = True
self.wait(300)
self.frame = np.array([[10, 10], [10, 10]], dtype=np.uint8)
self.wait(1000)
print("STOPPED ")
def __exit__(self, exc_type, exc_val, exc_tb):
self.end_work()
def cleanup(self):
self.end_work()
# self.cap.release()
def set_camera(self, fps=60, width=640, height=480):
self.stop_frames = True
self.wait(500)
self.cap.set(cv2.CAP_PROP_FPS, fps)
self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, width)
self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, height)
self.wait(500)
self.stop_frames = False
def set_camera_high_res(self):
self.set_camera(30, 1024, 720)
def set_camera_low_res(self):
self.set_camera(60, 320, 240)
def work_f(self):
self.stop_frames = False
while True:
if not self.stop_frames:
ret, frame = self.cap.read()
if ret:
self.frame = frame
self.time_frame = time.time()
# time.sleep(0.001)
# self.wait(1)
else:
time.sleep(0.01)
pass
def get_key(self, clear=True):
last = self.last_key
self.last_key = -1
return last
def get_frame(self):
return self.frame
def lidar_work(self):
angle = 0
index = 0
self.measurements = []
for i in range(15):
self.measurements.append([])
while self.process.poll() is None:
try:
data = str(
self.process.stdout.readline().decode("utf-8").strip())
except:
print("Reading data error!")
continue
# print(data)
angle_old = angle
try: # theta: 13.34 dist: 351.45 Q:47
angle = float(data[(data.find(":") + 2):(data.find(".") + 3)])
distance = float(data[(data.rfind(".") - 5):(data.rfind(".") +
3)])
# print(angle, " ", distance)
except:
print(data)
print("Parsing data error!")
continue
if angle < angle_old:
if len(self.measurements[len(self.measurements) - 1]) > index:
for i in range(
index,
len(self.measurements[len(self.measurements) -
1])):
self.measurements[len(self.measurements) - 1].pop()
index = 0
self.measurements.pop(0)
self.measurements.append([])
if index + 1 > len(self.measurements[len(self.measurements) - 1]):
self.measurements[len(self.measurements) - 1].append(
(angle, distance))
else:
self.measurements[len(self.measurements) -
1][index] = (angle, distance)
index += 1
self.isLidarWorking = False
def lidar_start(self):
if self.lidar is not None:
self.lidar.disconnect()
self.lidar = None
time.sleep(1)
if not self.isLidarWorking:
self.isLidarWorking = True
self.process = subprocess.Popen("C:/Robot/ultra_simple.exe //./" +
self.lidar_port,
stdout=subprocess.PIPE,
stderr=subprocess.PIPE,
shell=False)
while self.process.poll() is not None:
pass
thread = threading.Thread(target=self.lidar_work)
thread.daemon = True
thread.start()
def lidar_stop(self):
if self.process.poll() is None:
self.process.kill()
while self.process.poll() is None:
pass
self.lidar = RPLidar(self.lidar_port)
self.lidar.connect()
self.lidar.stop_motor()
self.lidar.stop()
def set_frame(self, frame):
fps = int(100 / (time.time() - self.fps_timer)) / 100
frame = self.text_to_frame(frame,
str(fps),
0,
20,
font_color=(0, 0, 255),
font_size=1)
cv2.imshow("frame", frame)
self.k = cv2.waitKey(1)
self.fps_timer = time.time()
return
@staticmethod
def wait(millis):
time.sleep(millis / 1000)
def send(self, message, flag_wait=True):
# print("send message")
self.port.write(bytes(message.encode('utf-8')))
self.port.write(bytes("|\n".encode('utf-8')))
# time.sleep(0.01)
answer = ""
print("Sent " + message)
if flag_wait:
print("Waiting for answer...")
while not self.port.in_waiting:
pass
while self.port.in_waiting:
answer = answer + str(self.port.readline())
print("Got answer: " + answer[2:len(answer) - 5])
return answer[2:len(answer) - 5] # удаляем \r\n
def move(self, m1, m2, time=0, encoder=0, wait=False):
m1 = self.constrain(m1, -255, 255)
m2 = self.constrain(m2, -255, 255)
self.motor_left = m1
self.motor_right = m2
if time != 0:
message = "MOVE_TIME|" + str(m1) + "|" + str(m2) + "|" + str(time)
elif encoder != 0:
message = "MOVE_ENCODER|" + str(m1) + "|" + str(m2) + "|" + str(
encoder)
else:
message = "MOVE|" + str(m1) + "|" + str(m2)
if wait:
message += "|WAIT"
print(message)
m = self.send(message)
if wait and (time != 0 or encoder != 0):
while not self.port.in_waiting: # пока входной буфер пуст
pass
# if wait:
# self.wait(time)
return m
def servo_yaw(self, angle):
angle = self.constrain(angle, -180, 180)
return self.send("SERVO_YAW|" + str(angle))
def servo_pitch(self, angle):
angle = self.constrain(angle, -180, 180)
return self.send("SERVO_PITCH|" + str(angle))
def distance(self):
s = self.send("DISTANCE")
s = s.split("|")
d = -1
try:
d = float(s[1])
except:
pass
return d
def voltage(self):
s = self.send("VOLTAGE")
# pos = s.find("VCC")
# s = s[pos:]
s = s.split("|")
v = -1
try:
v = float(s[1])
except:
pass
return v
@staticmethod
def millis():
return int(round(time.time() * 1000))
@staticmethod
def text_to_frame(frame,
text,
x,
y,
font_color=(255, 255, 255),
font_size=2):
cv2.putText(frame, str(text), (x, y), cv2.FONT_HERSHEY_COMPLEX_SMALL,
1, font_color, font_size)
return frame
def vcc_to_frame(self, frame):
return self.text_to_frame(frame, str(self.voltage()), 10, 20)
def dist_to_frame(self, frame):
return self.text_to_frame(frame, str(self.distance()), 550, 20)
@staticmethod
def distance_between_points(xa, ya, xb, yb, za=0, zb=0):
return np.sqrt(
np.sum((np.array((xa, ya, za)) - np.array((xb, yb, zb)))**2))
@staticmethod
def map(x, in_min, in_max, out_min, out_max):
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min
@staticmethod
def constrain(x, min, max):
if x < min:
return min
else:
if x > max:
return max
else:
return x
def manual(self, c=-1, show_code=False):
# m = c
# if c == -1:
# m = self.get_key()
# if self.flag_camera:
# frame = self.get_frame()
# if keyboard.is_pressed("m"):
# self.wait(200)
# if self.manual_mode == 0:
# print("manual on")
# self.manual_mode = 1
# else:
# print("manual off")
# self.manual_mode = 0
# if time.time() - self.t > 0.1:
# self.t = time.time()
# if keyboard.is_pressed('1'):
# if self.small_frame == 1:
# self.small_frame = 0
# else:
# self.small_frame = 1
#
# if self.manual_mode == 0:
# return self.manual_mode
#
# if self.manual_mode == 1:
# if keyboard.is_pressed('w'):
# self.move(self.manual_speed, self.manual_speed, wait=True)
# elif keyboard.is_pressed('s'):
# self.move(-self.manual_speed, -self.manual_speed, wait=True)
# elif keyboard.is_pressed('d'):
# self.move(self.manual_speed, -self.manual_speed, wait=True)
# elif keyboard.is_pressed('a'):
# self.move(-self.manual_speed, self.manual_speed, wait=True)
# else:
# self.move(0, 0, wait=True)
# # 75 72 77 80
# if keyboard.is_pressed("RIGHT"):
# print("75")
# self.manual_angle_yaw -= 10
# self.manual_angle_yaw = self.constrain(self.manual_angle_yaw, 0, 90)
# self.servo_yaw(self.manual_angle_yaw)
# if keyboard.is_pressed("LEFT"):
# print("72")
# self.manual_angle_yaw += 10
# self.manual_angle_yaw = self.constrain(self.manual_angle_yaw, 0, 90)
# self.servo_yaw(self.manual_angle_yaw)
# if keyboard.is_pressed("UP"):
# print("77")
# self.manual_angle_pitch -= 10
# self.manual_angle_pitch = self.constrain(self.manual_angle_pitch, 0, 90)
# self.servo_pitch(self.manual_angle_pitch)
# if keyboard.is_pressed("DOWN"):
# print("80")
# self.manual_angle_pitch += 10
# self.manual_angle_pitch = self.constrain(self.manual_angle_pitch, 0, 90)
# self.servo_pitch(self.manual_angle_pitch)
# if keyboard.is_pressed(' '):
# print("' '")
# self.manual_angle_pitch = 70
# self.manual_angle_yaw = 70
# self.servo_yaw(self.manual_angle_yaw)
# self.servo_pitch(self.manual_angle_pitch)
# if keyboard.is_pressed('-'):
# self.manual_speed -= 20
# if self.manual_speed < 50:
# self.manual_speed = 50
#
# if keyboard.is_pressed('+'):
# self.manual_speed += 20
# if self.manual_speed > 250:
# self.manual_speed = 250
#
# if keyboard.is_pressed('0'):
# if self.manual_video == 0:
# self.manual_video = 1
# else:
# self.manual_video = 0
if self.flag_camera:
frame = self.get_frame()
if self.k == ord("m"):
self.wait(200)
if self.manual_mode == 0:
print("manual on")
self.manual_mode = 1
else:
print("manual off")
self.manual_mode = 0
if time.time() - self.t > 0.1:
self.t = time.time()
if self.k == ord('1'):
if self.small_frame == 1:
self.small_frame = 0
else:
self.small_frame = 1
if self.manual_mode == 0:
return self.manual_mode
if self.manual_mode == 1:
if self.k == ord('w'):
self.move(self.manual_speed, self.manual_speed, wait=True)
elif self.k == ord('s'):
self.move(-self.manual_speed,
-self.manual_speed,
wait=True)
elif self.k == ord('d'):
self.move(self.manual_speed, -self.manual_speed, wait=True)
elif self.k == ord('a'):
self.move(-self.manual_speed, self.manual_speed, wait=True)
else:
self.move(0, 0, wait=True)
# 75 72 77 80
if keyboard.is_pressed("RIGHT"):
print("75")
self.manual_angle_yaw -= 10
self.manual_angle_yaw = self.constrain(
self.manual_angle_yaw, 0, 90)
self.servo_yaw(self.manual_angle_yaw)
if keyboard.is_pressed("LEFT"):
print("72")
self.manual_angle_yaw += 10
self.manual_angle_yaw = self.constrain(
self.manual_angle_yaw, 0, 90)
self.servo_yaw(self.manual_angle_yaw)
if keyboard.is_pressed("UP"):
print("77")
self.manual_angle_pitch -= 10
self.manual_angle_pitch = self.constrain(
self.manual_angle_pitch, 0, 90)
self.servo_pitch(self.manual_angle_pitch)
if keyboard.is_pressed("DOWN"):
print("80")
self.manual_angle_pitch += 10
self.manual_angle_pitch = self.constrain(
self.manual_angle_pitch, 0, 90)
self.servo_pitch(self.manual_angle_pitch)
if self.k == ord(' '):
print("' '")
self.manual_angle_pitch = 70
self.manual_angle_yaw = 70
self.servo_yaw(self.manual_angle_yaw)
self.servo_pitch(self.manual_angle_pitch)
if self.k == ord('-'):
self.manual_speed -= 20
if self.manual_speed < 50:
self.manual_speed = 50
if self.k == ord('+'):
self.manual_speed += 20
if self.manual_speed > 250:
self.manual_speed = 250
if self.k == ord('0'):
if self.manual_video == 0:
self.manual_video = 1
else:
self.manual_video = 0
# self.set_frame(
# self.dist_to_frame(self.vcc_to_frame(self.text_to_frame(frame, "manual", 280, 20))))
if self.manual_mode == 1 and self.flag_camera == 1:
if self.small_frame == 1:
frame = cv2.resize(frame, None, fx=0.25, fy=0.25)
self.set_frame(frame)
return self.manual_mode
# frame = self.dist_to_frame(self.vcc_to_frame(self.text_to_frame(
# self.text_to_frame(frame, str(self.manual_speed), 0, 100), "manual", 280, 20)))
frame = self.text_to_frame(
self.text_to_frame(frame, "manual", 280, 20),
str(self.manual_speed), 0, 100)
# frame = cv2.resize(frame, None, fx=0.5, fy=0.5, interpolation=cv2.INTER_CUBIC)
# frame = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
self.set_frame(frame)
return self.manual_mode
## fixDistanceToBlade(safeDistance, distDroneToBlade, vehicle)
## movementToPosition(nextX, nextY, droneP, vehicle)
## rotateDrone(0, False, vehicle)
#### condition_yaw(meanAngle_compensated)
## condition_yaw(angleOffsetMeasured)
if isArmed is True:
prevHeightMeasured = heightMeasured
elapsed = timeit.default_timer() - start_time
except KeyboardInterrupt:
print("Force stopped")
break
except RPLidarException:
print(RPLidarException.message)
## attempt = attempt + 1
continue
serial_lidar.stop()
serial_lidar.stop_motor()
serial_lidar.disconnect()
##vehicle.close()
serial_IO.close()
serial_IMU.close()
class Lidar_thread(Thread):
def __init__(self, bus):
Thread.__init__(self)
self.bus = bus
print(self.getName(), 'initialized')
self.lidar = RPLidar(PORT_NAME)
def run(self):
print(self.getName(), 'running')
i = 0
bestQuality = 0
bestDistance = 7000
oldGoodValue = 7000
Kp = 0.06
for new_scan, quality, angle, distance in self.lidar.iter_measurments(
):
#print(self.getName(), 'reading lidar') #OK
if VN.exit_lidar.isSet():
print(self.getName(), 'stopping')
# dans VarNairobi exit_lidar=threading.Even(); exit_lidar.clear()
# et dans le main on ajoute signal.signal(signal.sigint signal handler)???
time.sleep(5)
self.lidar.stop()
self.lidar.stop_motor()
self.lidar.disconnect()
break
else:
#dans VarNairobi wait_interface_on=threading.Even(); wait_interface_on.clear()
#Filtrage des donnees recuperees par le lidar
if quality != 0 and 172 <= angle and angle <= 188:
if quality > bestQuality:
bestQuality = quality
if distance < bestDistance:
bestDistance = int(distance)
print(self.getName(), ': BD : ', bestDistance)
i = 1
#Calcul de la cmd vitesse
elif angle > 198 and i == 1:
print(self.getName(), ': Test')
if VN.DistLidarSem.acquire(
False): #acquire semaphore without blocking
print(self.getName(), ': access DistLidar')
VN.DistLidar = bestDistance
VN.DistLidarSem.release()
else:
print(self.getName(), ': can not access DistLidar')
if VN.PlatooningActive.isSet():
if bestDistance > 4000:
if oldGoodValue < 3000:
temp = bestDistance
bestDistance = oldGoodValue
oldGoodValue = temp
else:
oldGoodValue = bestDistance
else:
oldGoodvalue = bestDistance
errorDistance = bestDistance - 2000
speed = (errorDistance * Kp)
speed = int(speed)
if speed <= 0:
speed = 0
elif speed >= 20:
speed = 20
if bestDistance >= 3000:
speed = 0
cmd_mv = (50 + speed) | 0x80
print(cmd_mv)
msg = can.Message(
arbitration_id=MCM,
data=[cmd_mv, cmd_mv, 0, 0, 0, 0, 0, 0],
extended_id=False)
self.bus.send(msg)
i = 0
bestQuality = 0
bestDistance = 7000
class PromobotAPI:
t = 0
measurements = []
process = None
lidar_port = "COM5"
def __init__(self, port='com3'):
self.lidar_port = port
def lidar_work(self):
angle = 0
index = 0
self.measurements = []
for i in range(15):
self.measurements.append([])
while self.process.poll() is None:
try:
data = str(
self.process.stdout.readline().decode("utf-8").strip())
except:
print("Reading data error!")
continue
# print(data)
angle_old = angle
try: # theta: 13.34 dist: 351.45 Q:47
angle = float(data[(data.find(":") + 2):(data.find(".") + 3)])
distance = float(data[(data.rfind(".") - 5):(data.rfind(".") +
3)])
# print(angle, " ", distance)
except:
print(data)
print("Parsing data error!")
continue
if angle < angle_old:
if len(self.measurements[len(self.measurements) - 1]) > index:
for i in range(
index,
len(self.measurements[len(self.measurements) -
1])):
self.measurements[len(self.measurements) - 1].pop()
index = 0
self.measurements.pop(0)
self.measurements.append([])
if index + 1 > len(self.measurements[len(self.measurements) - 1]):
self.measurements[len(self.measurements) - 1].append(
(angle, distance))
else:
self.measurements[len(self.measurements) -
1][index] = (angle, distance)
index += 1
self.isLidarWorking = False
def lidar_start(self):
if self.lidar is not None:
self.lidar.disconnect()
self.lidar = None
time.sleep(1)
if not self.isLidarWorking:
self.isLidarWorking = True
self.process = subprocess.Popen("C:/Robot/ultra_simple.exe //./" +
self.lidar_port,
stdout=subprocess.PIPE,
stderr=subprocess.PIPE,
shell=False)
while self.process.poll() is not None:
pass
thread = threading.Thread(target=self.lidar_work)
thread.daemon = True
thread.start()
def lidar_stop(self):
if self.process.poll() is None:
self.process.kill()
while self.process.poll() is None:
pass
self.lidar = RPLidar(self.lidar_port)
self.lidar.connect()
self.lidar.stop_motor()
self.lidar.stop()
class Lidar(BaseLidar):
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 stop(self):
# await super().stop()
self.lidar.stop()
self.lidar.stop_motor()
self.lidar.disconnect()
def run(self):
try:
previous_distances = None
previous_angles = None
iterator = self.lidar.iter_scans()
next(iterator)
while True:
items = [(q, 360 - angle, dist)
for q, angle, dist in next(iterator)]
distances = [item[2] for item in items]
angles = [item[1] for item in items]
if len(distances) > SLAM['MIN_SAMPLES']:
self.slam.update(distances, scan_angles_degrees=angles)
previous_distances = distances.copy()
previous_angles = angles.copy()
elif previous_distances is not None:
self.slam.update(previous_distances,
scan_angles_degrees=previous_angles)
x, y, theta = self.slam.getpos()
self.trajectory.append((x, y))
self.slam.getmap(self.mapbytes)
for coords in self.trajectory:
x_mm, y_mm = coords
x_pix = mm2pix(x_mm)
y_pix = mm2pix(y_mm)
self.mapbytes[y_pix * SLAM['MAP_SIZE_PIXELS'] + x_pix] = 0
checksum = sum(self.mapbytes)
if self.on_map_change and checksum != self.prev_checksum:
# print(checksum)
x = Image.frombuffer(
'L', (self.map_size_pixels, self.map_size_pixels),
self.mapbytes, 'raw', 'L', 0, 1)
bytes_img = io.BytesIO()
x.save(bytes_img, format='PNG')
self.on_map_change(bytearray(bytes_img.getvalue()))
self.prev_checksum = checksum
except Exception as e:
print(e)
finally:
self.stop()
def run(path):
'''Main function'''
loop = False
Inches = None
lidarDegrees = 0
reset = 27463478276547
baseMeasurement = 18000
ultimateMeasurement = 6400
PORT_NAME = '/dev/ttyUSB0'
lidar = RPLidar(PORT_NAME)
outfile = open(path, 'w')
stop = False
lidarToBack = 0
try:
print('Recording measures... Press Crl+C to stop.')
for measurment in lidar.iter_measures():
distance = measurment[3]
degrees = measurment[2]
if (degrees <= 300 and degrees >= 200):
loop = True
if (degrees >= 330 or degrees <= 30) and distance != 0:
#print '[ultimateMeasurement %f]' % (ultimateMeasurement)
#print '[degrees %f]' % (degrees)
#print '[distance %f]' % (distance)
#if distance < baseMeasurement:
#ultimateMeasurement = distance
if distance <= ultimateMeasurement:
ultimateMeasurement = distance
lidarDegrees = degrees
if loop == True:
loop = False
Inches = (ultimateMeasurement / 25.4) - lidarToBack
print "[inches %f]", (Inches)
ultimateMeasurement = baseMeasurement
if Inches is None:
continue
#note: in all situations below, left should be listed above right
if Inches <= 15 and Inches >= 9:
stop = True
sd.putBoolean("Stop", stop)
print("Boolean Stop is True")
else:
stop = False
sd.putBoolean("Stop", stop)
print("Boolean Stop is False")
#print'[robotSpeed = %f]' % (robotSpeed)
#sd.putNumber('ultimateMeasurement', ultimateMeasurement)
#if loop == True:
#ultimateMeasurement = baseMeasurement
except KeyboardInterrupt:
print('Stopping.')
except:
traceback.print_exc()
lidar.stop_motor()
lidar.stop()
lidar.disconnect()
outfile.close()
def findexactvanes(lower_angle_L, lower_angle_R, upper_angle_L, upper_angle_R,
lower_distance_L, lower_distance_R, upper_distance_L,
upper_distance_R):
mylidar = RPLidar("COM3", baudrate=115200)
mylidar_scan = []
totalaverageleftvane = []
totalaveragerightvane = []
totalaverageangleleftvane = []
totalaverageanglerightvane = []
for y in range(0, 5):
for i, scan in enumerate(
mylidar.iter_scans()): #scan_type='normal', max_buf_meas=60000
# print('%d: Got %d measures' % (i, len(scan)))
#
mylidar_scan.append(scan)
if i > 10:
break
for i in range(len(mylidar_scan)): # aantal rondes
leftvane = []
rightvane = []
# print("Len lidarscan i : ", len(mylidar_scan[i]))
for j in range(len(
mylidar_scan[i])): # aantal metingen in het rondje
mylist = mylidar_scan[i][j]
if lower_angle_L < mylist[
1] < upper_angle_L and lower_distance_L < mylist[
2] < upper_distance_L:
leftvane.append(mylist)
elif lower_angle_R < mylist[
1] < upper_angle_R and lower_distance_R < mylist[
2] < upper_distance_R:
rightvane.append(mylist)
# print("arr_avg: ", arr_avg)
if leftvane and rightvane:
leftvane = np.array(leftvane)
rightvane = np.array(rightvane)
averageleftvane = np.mean(leftvane[:, 2])
averagerightvane = np.mean(rightvane[:, 2])
averageangleleftvane = np.mean(leftvane[:, 1])
averageanglerightvane = np.mean(rightvane[:, 1])
totalaverageleftvane.append(averageleftvane)
totalaveragerightvane.append(averagerightvane)
totalaverageangleleftvane.append(averageangleleftvane)
totalaverageanglerightvane.append(averageanglerightvane)
print("Average numpy left", averageleftvane)
print("Average numpy right", averagerightvane)
#mylidar.clean_input()
grandtotalleft = np.mean(totalaverageleftvane)
grandtotalright = np.mean(totalaveragerightvane)
grandtotalleftangle = np.mean(totalaverageangleleftvane)
grandtotalrightangle = np.mean(totalaverageanglerightvane)
print("totaal links:", grandtotalleft)
print("totaal rechts:", grandtotalright)
print("totaal hoek links:", grandtotalleftangle)
print("totaal hoek rechts:", grandtotalrightangle)
mylidar.stop()
mylidar.stop_motor()
mylidar.disconnect()
return grandtotalleft, grandtotalright, grandtotalleftangle, grandtotalrightangle
def run(lower_angle_L, lower_angle_R, upper_angle_L, upper_angle_R,
lower_distance_L, lower_distance_R, upper_distance_L,
upper_distance_R):
mylidar = RPLidar(PORT_NAME, baudrate=115200)
total = 0
arr_avg_L = []
arr_avg_R = []
avg_angle_L = []
avg_angle_R = []
mylidar_scan = []
angle_L = 0
angle_R = 0
dist_L = 0
dist_R = 0
for y in range(0, 15):
for i, scan in enumerate(
mylidar.iter_scans(scan_type='normal', max_buf_meas=60000)):
# print('%d: Got %d measures' % (i, len(scan)))
#
mylidar_scan.append(scan)
if i > 10:
break
for i in range(len(mylidar_scan)): # aantal rondes
# print("Len lidarscan i : ", len(mylidar_scan[i]))
for j in range(len(
mylidar_scan[i])): # aantal metingen in het rondje
mylist = mylidar_scan[i][j]
# print(mylist[2])
if lower_angle_L < mylist[
1] < upper_angle_L and lower_distance_L < mylist[
2] < upper_distance_L:
dist_L = mylist[2]
angle_L = mylist[1]
total = 1
elif lower_angle_R < mylist[
1] < upper_angle_R and lower_distance_R < mylist[
2] < upper_distance_R:
dist_R = mylist[2]
angle_R = mylist[1]
total = 1
if total == 1:
# aver = som / total
avg_angle_L.append(angle_L)
avg_angle_R.append(angle_R)
arr_avg_L.append(dist_L)
arr_avg_R.append(dist_R)
total = 0
# print("arr_avg: ", arr_avg)
arr_mean_L = np.mean(arr_avg_L)
arr_mean_R = np.mean(arr_avg_R)
arr_avg_angle_L = np.mean(avg_angle_L)
arr_avg_angle_R = np.mean(avg_angle_R)
print("Average Left: ", arr_mean_L)
print("Average Right: ", arr_mean_R)
mylidar.clean_input()
mylidar.stop()
mylidar.stop_motor()
mylidar.disconnect()
return arr_mean_L, arr_mean_R, arr_avg_angle_L, arr_avg_angle_R
class PromobotAPI:
port = None
server_flag = False
last_frame = np.array([[10, 10], [10, 10]], dtype=np.uint8)
quality = 50
manual_mode = 0
manual_video = 1
manual_speed = 150
manual_angle_yaw = 70
manual_angle_pitch = 70
frame = []
small_frame = 0
motor_left = 0
motor_right = 0
encode_param = 0
flag_serial = False
flag_camera = False
t = 0
measurements = []
process = None
lidar_port = "COM5"
lidar = None
isLidarWorking = False
k = 0
fps_timer = 0
audio_queue = []
thread_listening = None
stop_listen = True
recognizer = None
text_queue = []
flag_speech = True
flag_tts = True
flag_lidar = True
voices = [
"HKEY_LOCAL_MACHINE\SOFTWARE\Microsoft\Speech\Voices\Tokens\TTS_MS_RU-RU_IRINA_11.0",
"HKEY_LOCAL_MACHINE\SOFTWARE\Microsoft\Speech\Voices\Tokens\TTS_MS_EN-US_ZIRA_11.0",
"HKEY_LOCAL_MACHINE\SOFTWARE\Microsoft\Speech\Voices\Tokens\TTS_MS_EN-US_DAVID_11.0"
]
voice_engine = None
tts_queue = []
tts_work = False
servo_yaw_angle = 0
servo_pitch_angle = 0
last_key = 0
def __init__(self,
flag_serial=True,
flag_camera=True,
flag_speech=True,
flag_tts=True,
flag_lidar=True,
voice_type=0):
# print("\x1b[42m" + "Start script" + "\x1b[0m")
self.flag_camera = flag_camera
self.flag_speech = flag_speech
self.flag_serial = flag_serial
self.flag_tts = flag_tts
self.flag_lidar = flag_lidar
print("Start script")
if flag_tts:
self.voice_engine = pyttsx3.init()
self.voice_engine.setProperty('voice', self.voices[voice_type])
if flag_speech:
self.recognizer = sr.Recognizer()
# print("open robot port")
if flag_serial:
self.port = serial.Serial('COM3', 500000, timeout=2)
time.sleep(1.5)
if self.send("CONNECT").__contains__("OK"):
print(colored("Arduino connected", "green"))
else:
print(colored("Arduino connect error", "red"))
if flag_camera:
self.cap = cv2.VideoCapture()
self.cap.open(0)
r, self.frame = self.cap.read()
self.time_frame = time.time()
if self.flag_serial:
self.servo_yaw(70)
self.servo_pitch(70)
if self.flag_lidar:
self.lidar_start()
self.lidar_stop()
self.stop_frames = False
if flag_camera:
self.my_thread_f = threading.Thread(target=self.work_f)
self.my_thread_f.daemon = True
self.my_thread_f.start()
self.manual_video = 1
pass
def end_work(self):
# self.cap.release()
self.move(0, 0)
self.stop_frames = True
self.wait(300)
self.frame = np.array([[10, 10], [10, 10]], dtype=np.uint8)
self.wait(1000)
print("STOPPED ")
def __exit__(self, exc_type, exc_val, exc_tb):
self.end_work()
def cleanup(self):
self.end_work()
# self.cap.release()
def set_camera(self, fps=60, width=640, height=480):
self.stop_frames = True
self.wait(500)
self.cap.set(cv2.CAP_PROP_FPS, fps)
self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, width)
self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, height)
self.wait(500)
self.stop_frames = False
def set_camera_high_res(self):
self.set_camera(30, 1024, 720)
def set_camera_low_res(self):
self.set_camera(60, 320, 240)
def work_f(self):
self.stop_frames = False
while True:
if not self.stop_frames:
ret, frame = self.cap.read()
if ret:
self.frame = frame
self.time_frame = time.time()
# time.sleep(0.001)
# self.wait(1)
else:
time.sleep(0.01)
pass
def get_frame(self):
return self.rotate_image(self.frame, 180)
@staticmethod
def rotate_image(image, angle):
image_center = tuple(np.array(image.shape[1::-1]) / 2)
rot_mat = cv2.getRotationMatrix2D(image_center, angle, 1.0)
result = cv2.warpAffine(image,
rot_mat,
image.shape[1::-1],
flags=cv2.INTER_LINEAR)
return result
def lidar_work(self):
angle = 0
index = 0
self.measurements = []
for i in range(15):
self.measurements.append([])
while self.process.poll() is None:
try:
data = str(
self.process.stdout.readline().decode("utf-8").strip())
except:
print("Reading data error!")
continue
# print(data)
angle_old = angle
try: # theta: 13.34 dist: 351.45 Q:47
angle = float(data[(data.find(":") + 2):(data.find(".") + 3)])
distance = float(data[(data.rfind(".") - 5):(data.rfind(".") +
3)])
# print(angle, " ", distance)
except:
print(data)
print("Parsing data error!")
continue
if angle < angle_old:
if len(self.measurements[len(self.measurements) - 1]) > index:
for i in range(
index,
len(self.measurements[len(self.measurements) -
1])):
self.measurements[len(self.measurements) - 1].pop()
index = 0
self.measurements.pop(0)
self.measurements.append([])
if index + 1 > len(self.measurements[len(self.measurements) - 1]):
self.measurements[len(self.measurements) - 1].append(
(angle, distance))
else:
self.measurements[len(self.measurements) -
1][index] = (angle, distance)
index += 1
self.isLidarWorking = False
def lidar_start(self):
if self.flag_lidar:
if self.lidar is not None:
self.lidar.disconnect()
self.lidar = None
time.sleep(1)
if not self.isLidarWorking:
self.isLidarWorking = True
self.process = subprocess.Popen(
"C:/Robot/ultra_simple.exe //./" + self.lidar_port,
stdout=subprocess.PIPE,
stderr=subprocess.PIPE,
shell=False)
while self.process.poll() is not None:
pass
thread = threading.Thread(target=self.lidar_work)
thread.daemon = True
thread.start()
def lidar_stop(self):
if self.process.poll() is None:
self.process.kill()
while self.process.poll() is None:
pass
self.lidar = RPLidar(self.lidar_port)
self.lidar.connect()
self.lidar.stop_motor()
self.lidar.stop()
def listening(self):
while True:
if len(self.audio_queue) > 0:
try:
audio = self.audio_queue.pop()
print("Распознавание...")
t = time.time()
text = self.recognizer.recognize_google(audio,
language="ru-RU")
self.text_queue.append(text)
print(
self.recognizer.recognize_google(audio,
language="ru-RU"))
print("Recognition time = ", time.time() - t)
except sr.UnknownValueError:
print("Робот не расслышал фразу")
continue
except sr.RequestError as e:
print("Ошибка сервиса; {0}".format(e))
def start_listening(self):
if self.flag_speech:
if self.thread_listening is not None:
if not self.thread_listening.isAlive():
self.stop_listen = False
self.thread_listening = threading.Thread(
target=self.listening)
self.thread_listening.daemon = True
self.thread_listening.start()
def stop_listening(self):
if self.thread_listening is not None:
if not self.thread_listening.isAlive():
self.stop_listen = True
def is_voice_command(self):
return len(self.text_queue)
def get_voice_command(self):
if self.is_voice_command():
return self.text_queue.pop()
else:
return -1
def run_tts(self):
self.voice_engine.runAndWait()
def tts_working(self):
self.tts_work = True
print("working")
while True:
if len(self.tts_queue) > 0:
if not self.voice_engine.isBusy():
text, wait = self.tts_queue.pop()
print("say")
self.voice_engine.say(text)
if wait:
self.voice_engine.runAndWait()
else:
thread = threading.Thread(target=self.run_tts)
thread.daemon = True
thread.start()
else:
time.sleep(0.2)
else:
time.sleep(0.2)
self.tts_work = False
def say(self, text, wait=False):
if self.flag_tts:
print("start")
self.tts_queue.append((text, wait))
if not self.tts_work:
self.tts_work = True
thread = threading.Thread(target=self.tts_working)
thread.daemon = True
thread.start()
def set_frame(self, frame):
try:
fps = int(100 / (time.time() - self.fps_timer)) / 100
except ZeroDivisionError:
fps = 500
self.fps_timer = time.time()
frame = self.text_to_frame(frame,
str(fps),
0,
20,
font_color=(0, 0, 255),
font_size=1)
cv2.imshow("frame", frame)
self.k = cv2.waitKey(1)
return
@staticmethod
def wait(millis):
time.sleep(millis / 1000)
def send(self, message, flag_wait=True):
# print("send message")
while self.port.in_waiting:
self.port.readline()
try:
self.port.write(bytes(message.encode('utf-8')))
except serial.SerialTimeoutException:
print(colored("SerialTimeoutException", "red"))
time.sleep(0.001)
try:
self.port.write(bytes("|\n".encode('utf-8')))
except serial.SerialTimeoutException:
print(colored("SerialTimeoutException"), "red")
# time.sleep(0.01)
answer = ""
print("Sent " + message)
if flag_wait:
print("Waiting for answer...")
while not self.port.in_waiting:
pass
# while self.port.in_waiting:
answer = str(self.port.read_until(bytes('\n'.encode('utf-8'))))
# answer = answer + str(self.port.readline())
print("Got answer: " + answer[2:len(answer) - 5])
return answer[2:len(answer) - 5] # удаляем \r\n
def move(self, m1, m2, time=0, encoder=0, wait=False):
m1 = self.constrain(m1, -255, 255)
m2 = self.constrain(m2, -255, 255)
self.motor_left = m1
self.motor_right = m2
if time != 0:
message = "MOVE_TIME|" + str(m1) + "|" + str(m2) + "|" + str(time)
elif encoder != 0:
message = "MOVE_ENCODER|" + str(m1) + "|" + str(m2) + "|" + str(
encoder)
else:
message = "MOVE|" + str(m1) + "|" + str(m2)
if wait:
message += "|WAIT"
print(message)
m = self.send(message)
if wait and (time != 0 or encoder != 0):
print("move waiting...")
while not self.port.in_waiting: # пока входной буфер пуст
pass
ans = ""
# while self.port.in_waiting:
# ans += str(self.port.readline())
ans = str(self.port.read_until(bytes("\n".encode('utf-8'))))
print("move_time end ", ans)
# if wait:
# self.wait(time)
return m
def servo_yaw(self, angle, absolute=True):
if absolute:
self.servo_yaw_angle = angle
else:
self.servo_yaw_angle += angle
self.servo_yaw_angle = self.constrain(self.servo_yaw_angle, 0, 180)
return self.send("SERVO_YAW|" + str(self.servo_yaw_angle))
def servo_pitch(self, angle, absolute=True):
if absolute:
self.servo_pitch_angle = angle
else:
self.servo_pitch_angle += angle
self.servo_pitch_angle = self.constrain(self.servo_pitch_angle, 0, 180)
return self.send("SERVO_PITCH|" + str(self.servo_pitch_angle))
def voltage(self):
s = self.send("VOLTAGE")
# pos = s.find("VCC")
# s = s[pos:]
s = s.split("|")
v = -1
try:
v = float(s[1])
except:
pass
return v
@staticmethod
def millis():
return int(round(time.time() * 1000))
@staticmethod
def text_to_frame(frame,
text,
x,
y,
font_color=(255, 255, 255),
font_size=2):
cv2.putText(frame, str(text), (x, y), cv2.FONT_HERSHEY_COMPLEX_SMALL,
1, font_color, font_size)
return frame
def vcc_to_frame(self, frame):
return self.text_to_frame(frame, str(self.voltage()), 10, 20)
def dist_to_frame(self, frame):
return self.text_to_frame(frame, str(self.distance), 550, 20)
@staticmethod
def distance_between_points(xa, ya, xb, yb, za=0, zb=0):
return np.sqrt(
np.sum((np.array((xa, ya, za)) - np.array((xb, yb, zb)))**2))
@staticmethod
def map(x, in_min, in_max, out_min, out_max):
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min
@staticmethod
def constrain(x, min, max):
if x < min:
return min
else:
if x > max:
return max
else:
return x
def manual(self):
if self.flag_camera:
frame = self.get_frame()
if self.k == ord("m"):
self.wait(150)
if self.manual_mode == 0:
print("manual on")
self.manual_mode = 1
else:
print("manual off")
self.move(0, 0)
self.manual_mode = 0
if self.manual_mode == 1:
if self.k == ord('w'):
self.move(self.manual_speed, self.manual_speed, 50, wait=False)
elif self.k == ord('s'):
self.move(-self.manual_speed,
-self.manual_speed,
50,
wait=False)
elif self.k == ord('d'):
self.move(self.manual_speed,
-self.manual_speed,
50,
wait=False)
elif self.k == ord('a'):
self.move(-self.manual_speed,
self.manual_speed,
50,
wait=False)
if time.time() - self.t > 0.1:
self.t = time.time()
if self.k == ord('1'):
if self.small_frame == 1:
self.small_frame = 0
else:
self.small_frame = 1
if self.manual_mode == 0:
return self.manual_mode
if self.manual_mode == 1:
# if self.k == ord('w'):
# self.move(self.manual_speed, self.manual_speed, 100, wait=True)
# elif self.k == ord('s'):
# self.move(-self.manual_speed, -self.manual_speed, 100, wait=True)
# elif self.k == ord('d'):
# self.move(self.manual_speed, -self.manual_speed, 100, wait=True)
# elif self.k == ord('a'):
# self.move(-self.manual_speed, self.manual_speed, 100, wait=True)
# elif self.last_key == ord('w') or self.last_key == ord('a')\
# or self.last_key == ord('s') or self.last_key == ord('d'):
# self.move(0, 0, wait=True)
# 75 72 77 80
if keyboard.is_pressed("RIGHT"):
print("75")
self.servo_yaw(-10, absolute=False)
if keyboard.is_pressed("LEFT"):
print("72")
self.servo_yaw(10, absolute=False)
if keyboard.is_pressed("UP"):
print("77")
self.servo_pitch(-10, absolute=False)
if keyboard.is_pressed("DOWN"):
print("80")
self.servo_pitch(10, absolute=False)
if self.k == ord(' '):
print("' '")
self.manual_angle_pitch = 70
self.manual_angle_yaw = 70
self.servo_yaw(self.manual_angle_yaw)
self.servo_pitch(self.manual_angle_pitch)
if self.k == ord('-'):
self.manual_speed -= 20
if self.manual_speed < 50:
self.manual_speed = 50
if self.k == ord('+'):
self.manual_speed += 20
if self.manual_speed > 250:
self.manual_speed = 250
if self.k == ord('0'):
if self.manual_video == 0:
self.manual_video = 1
else:
self.manual_video = 0
self.last_key = self.k
if self.manual_mode == 1 and self.flag_camera == 1:
if self.small_frame == 1:
frame = cv2.resize(frame, None, fx=0.25, fy=0.25)
self.set_frame(frame)
return self.manual_mode
# frame = self.dist_to_frame(self.vcc_to_frame(self.text_to_frame(
# self.text_to_frame(frame, str(self.manual_speed), 0, 100), "manual", 280, 20)))
frame = self.text_to_frame(
self.text_to_frame(frame, "manual", 280, 20),
str(self.manual_speed), 0, 100)
# frame = cv2.resize(frame, None, fx=0.5, fy=0.5, interpolation=cv2.INTER_CUBIC)
# frame = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
self.set_frame(frame)
return self.manual_mode
from rplidar import RPLidar
rp = RPLidar('/dev/ttyUSB0')
rp.stop()
rp.stop_motor()
rp.disconnect()
del rp
def LidarAlign360(path):
'''Main function'''
loop = False
Inches = None
lidarDegrees = 0
reset = 27463478276547
baseMeasurement = 2540
ultimateMeasurement = 6400
PORT_NAME = '/dev/ttyUSB0'
lidar = RPLidar(PORT_NAME)
outfile = open(path, 'w')
try:
print('Recording measures... Press Crl+C to stop.')
for measurment in lidar.iter_measures():
distance = measurment[3]
degrees = measurment[2]
if (degrees <= 185 and degrees >= 175):
loop = True
if (degrees >= 0 or degrees <= 360) and distance != 0:
#print '[ultimateMeasurement %f]' % (ultimateMeasurement)
#print '[degrees %f]' % (degrees)
#print '[distance %f]' % (distance)
#if distance < baseMeasurement:
#ultimateMeasurement = distance
if distance <= ultimateMeasurement:
ultimateMeasurement = distance
lidarDegrees = degrees
if loop == True:
loop = False
Inches = ultimateMeasurement / 25.4
print "[inches %f]", (Inches)
ultimateMeasurement = baseMeasurement
if Inches is None:
continue
#note: in all situations below, left should be listed above right
if Inches <= 100:
robotSpeed = (0.00005018 * (Inches * Inches)) + 0.1
elif Inches > 100:
robotSpeed = 0.5
print("robotSpeed", robotSpeed)
if lidarDegrees > 270 and lidarDegrees < 355 and Inches > 12:
left = robotSpeed
right = robotSpeed + 0.1
elif lidarDegrees > 5 and lidarDegrees < 90 and Inches > 12:
left = robotSpeed + 0.1
right = robotSpeed
elif lidarDegrees >= 90 and lidarDegrees < 175 and Inches > 12:
left = -1 * (robotSpeed + 0.1)
right = robotSpeed
elif lidarDegrees > 185 and lidarDegrees < 270 and Inches > 12:
left = robotSpeed
right = -1 * (robotSpeed + .1)
elif lidarDegrees >= 175 and lidarDegrees <= 185 and Inches >= 36:
left = -1 * robotSpeed
right = -1 * robotSpeed
print("back", robotSpeed)
elif lidarDegrees >= 355 and lidarDegrees <= 5 and Inches >= 12:
left = robotSpeed
right = robotSpeed
elif Inches <= 6 and lidarDegrees >= 270 and lidarDegrees <= 90:
print 'stopped'
right = 0
left = 0
elif Inches <= 36 and lidarDegrees >= 90 and lidarDegrees <= 270:
print 'stopped'
right = 0
left = 0
else:
left = 0.5
right = 0.5
sd.putNumber("left", left)
sd.putNumber("right", right)
print("lidarDegrees", lidarDegrees)
print("right", right)
print("left", left)
#print'[robotSpeed = %f]' % (robotSpeed)
#sd.putNumber('ultimateMeasurement', ultimateMeasurement)
#if loop == True:
#ultimateMeasurement = baseMeasurement
except KeyboardInterrupt:
print('Stopping.')
except:
traceback.print_exc()
lidar.stop_motor()
lidar.stop()
lidar.disconnect()
outfile.close()
import time
from rplidar import RPLidar
lidar = RPLidar('/dev/ttyUSB0')
info = lidar.get_info()
print(info)
health = lidar.get_health()
print(health)
lidar.stop()
lidar.stop_motor()
time.sleep(1)
#lidar.disconnect()
def stopLidar():
lidar = RPLidar(PORT_NAME)
lidar.stop()
lidar.stop_motor()
lidar.disconnect()
