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()
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 connect_to_lidar():
lidar = None
for n in range(0, 4):
try:
port = '/dev/ttyUSB%s' % n
print('Searchin RPLidar on port %s ... ' % port, end='', flush=1)
lidar = RPLidar(port)
lidar.connect()
print('done')
return lidar
except RPLidarException:
print('fail!')
def object_in_range(port, angle, distance):
lidar = RPLidar(port)
lidar.connect()
for scan in lidar.iter_measurments():
a = int(scan[2])
d = int(scan[3] / 10)
if ((a <= angle / 2) or (a >= 360 - angle / 2)):
if ((d <= distance) and (d != 0)):
break
lidar.disconnect()
return True
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'
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()
plt.xticks(x, range(-6, 7))
plt.yticks(y, range(-6, 7))
# Add some labels
plt.xlabel("X-Location (meters)")
plt.ylabel("Y-Location (meters)")
ax.add_patch(radius_view)
ax.add_patch(roomba)
if x_y is not None:
ax.plot(x_y[0], x_y[1], 'o')
if __name__ == '__main__':
lidar = RPLidar('/dev/ttyUSB0')
lidar.connect()
print lidar.get_health()
print lidar.get_device_info()
lidar.start_scan()
time.sleep(2) # Let that baby warm up
set_plot()
plt.ion()
plt.show()
try:
while True:
point_list = [p for p in lidar._raw_points]
rp_point_list = [
RPLidarPoint(raw_point) for raw_point in point_list
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)
elif args.front == 2:
ax.plot([-1 * (x) for x in x_y[0]], x_y[1], "-" if args.plot_lines else "o")
elif args.front == 3:
ax.plot([-1 * (y) for y in x_y[1]], [-1 * (x) for x in x_y[0]], "-" if args.plot_lines else "o")
elif args.front == 4:
ax.plot(x_y[0], [-1 * (y) for y in x_y[1]], "-" if args.plot_lines else "o")
# else:
# ax.plot(x_y[0], x_y[1], 'o') # This is just for debugging what is wrong with the above TODO
if __name__ == "__main__":
parse_args()
print args.front, type(args.front)
lidar = RPLidar(args.port_name)
lidar.connect()
print lidar.get_health()
print lidar.get_device_info()
lidar.start_scan()
time.sleep(2) # Let that baby warm up
set_plot()
plt.ion()
plt.show()
try:
while True:
point_list = [p for p in lidar._raw_points]
rp_point_list = [RPLidarPoint(raw_point) for raw_point in point_list]
rp_point_list.sort(key=lambda v: v.angle)
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
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
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()
