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()