def __init__(self, const_throttle=None): super(AutoDriftEnv, self).__init__() self.const_throttle = const_throttle # save the last n commands (throttle and steering) self.n_commands = 2 # interface to the car self.viewer = CarController() # action space if const_throttle is None: # define independent bounds for steering and throttle self.action_space = spaces.Box(low=np.array([-MAX_STEERING, -1]), high=np.array([MAX_STEERING, 1]), dtype=np.float32) else: # steering only self.action_space = spaces.Box(low=np.array([-MAX_STEERING]), high=np.array([MAX_STEERING]), dtype=np.float32) # use pixels as input self.observation_space = spaces.Box(low=0, high=255, shape=INPUT_DIM, dtype=np.uint8)
def car_control_open(self, *args): ''' open car camera and connect the car controller. :return: ''' print(">>> loading.") self.host_detector.detect_hosts() self.stream.pull() # call pull to init car camera. # check ESP 8266 connection if self.controller is None: controller_ip = None print("Found hosts:", self.host_detector.hosts) for host in self.host_detector.hosts: if 'esp' in host or 'ESP' in host: controller_ip = self.host_detector.hosts[host] break # TODO: seems ESP8266 can't be detect # used static IP for debug. # IP is pre-found from router configuration. if controller_ip is None: controller_ip = '192.168.43.213' # debug end. if controller_ip is not None: self.controller = CarController(controller_ip) self.controller.status = 'online' # TODO: check connection # self.controller.test() else: print(">>> ERROR: can't find ESP8266. Current hosts: ", self.host_detector.hosts) msg = QtWidgets.QMessageBox.warning( self, u"Warning", u"请检测汽车芯片ESP8266是否正确连接路由", buttons=QtWidgets.QMessageBox.Ok, defaultButton=QtWidgets.QMessageBox.Ok) else: # had connected before. self.controller.status = 'online' # open car video stream frame = self.stream.read_buffer() if frame is not None: if not self.timer_stream.isActive(): self.timer_stream.start(30) self.timer_stream.timeout.connect(self.stream_show)
def main(): pygame.init() size = width, height = 800, 800 d_surf = pygame.display.set_mode(size) clock = pygame.time.Clock() car_id = 'CAR_ID' wheel_id = 'WHEEL_ID' lot_id = 'LOT_ID' car_start_pos = (400, 600) car_length = 90 car_width = 50 car_angle = 0 wheel_angle = 0 image_manager = ImageManager() image_manager.put(car_id, 'images/car.png', (64, 112)) image_manager.put(wheel_id, 'images/tyre.png', (8, 24)) image_manager.put(lot_id, 'images/lot.png', (800, 800)) car = Car(car_id, wheel_id, car_start_pos, car_length, car_width, car_angle, wheel_angle) car_controller = CarController(car) car_drawer = CarDrawer(image_manager) while True: delta_ms = clock.tick(30) for event in pygame.event.get(): if event.type == pygame.QUIT: pygame.quit() sys.exit() elif event.type in (pygame.KEYUP, pygame.KEYDOWN): car_controller.handle_event(event) car_controller.update() car.update() d_surf.blit(image_manager.get(lot_id), (0, 0)) car_drawer.draw(d_surf, car) pygame.display.update()
#window.debug_menu.enabled = False window.size=(width,height) #Light(type='ambient', color=(0.3,0.3,0.3,1), direction=(1,1,1)) #ground = Entity(model='plane',shader=lit_with_shadows_shader, texture='white_cube',scale=(32,1,32),texture_scale=(32,32), collider='mesh',color=color.white) ground = Entity(model='plane', texture='road',scale=(1000,1,1000), texture_scale=(500,500), collider='box')#,shader=lit_with_shadows_shader) #ground = Entity(model='plane', scale=(20,1,20),texture_scale=(200,200), texture='road', collider='box') sun = DirectionalLight(y=10, rotation=(160,90,0)) #sun._light.show_frustum() Sky(color=color.rgb(200,200, 220, a=255) ) #player = FirstPersonController(model='cube', y=1, origin_y=-.5) player=CarController(y=10,x=2) def update(): pass app.run()
window.color = color.black window.exit_button.enabled = False window.fps_counter.enabled = True window.cog_button.enabled = False #window.debug_menu.enabled = False if mode == "test": window.position = (0, 20) window.borderless = False else: window.size = (width, height) # if hosting == True: window.title = "host" player = CarController(y=0.1, x=10, z=10) code = createMap(width=10, height=10, roads=10) gameSettings[1] = code displayMap(code) elif hosting == False: window.title = "client" player = CarController(y=0.1, x=10, z=10) #Light(type='ambient', color=(0.3,0.3,0.3,1), direction=(1,1,1)) #ground = Entity(model='plane',shader=lit_with_shadows_shader, texture='white_cube',scale=(32,1,32),texture_scale=(32,32), collider='mesh',color=color.white) #ground = Entity(model='plane',texture="track", scale=(200,1,200), collider='box',shader=lit_with_shadows_shader) #ground = Entity(model='plane', scale=(20,1,20),texture_scale=(200,200), texture='road', collider='box') #my_scene = load_blender_scene('gmae') ground = Entity( model='plane',
def __init__(self, cv_bridge, debug_stream=None): ''' Init ros node to receive images ''' self.cv_bridge = cv_bridge self.debug_stream = debug_stream if self.debug_stream: self.debug_stream.create_stream('rgb', 'debug/rgb') self.debug_stream.create_stream('depth', 'debug/depth') # ================ Initialize controlling models ================ self.image_queue = Queue.Queue(maxsize=config.IMAGE_QUEUE_SIZE) self.depth_image_queue = Queue.Queue(maxsize=config.IMAGE_QUEUE_SIZE) # Segmentation seg_config = path.join(DATA_FOLDER, 'semantic_seg_ENet.conf.json') self.semantic_segmentation = SemanticSegmentation( seg_config, debug_stream=debug_stream) # Depth processor self.depth_processor = DepthProcessor(debug_stream=debug_stream) # Sign detector self.image_processor = SignDetector(cv_bridge) # Car controlling self.car_controller = CarController(self.semantic_segmentation, debug_stream=debug_stream) self.sign_detector = SignDetector(cv_bridge) # Setup pub/sub # WTF BUG!!! https://answers.ros.org/question/220502/image-subscriber-lag-despite-queue-1/ self.rgb_camera_sub = rospy.Subscriber( config.TOPIC_GET_IMAGE, CompressedImage, callback=self.callback_rgb_image, queue_size=1, buff_size=2**24) if config.USE_DEPTH_CAMERA: self.depth_camera_sub = rospy.Subscriber( config.TOPIC_GET_DEPTH_IMAGE, CompressedImage, callback=self.callback_depth_image, queue_size=1, buff_size=2**24) # Setup workers control_worker = Thread(target=self.thread_car_control, args=(self.image_queue, self.depth_image_queue)) control_worker.setDaemon(True) control_worker.start() sign_worker = Thread(target=self.sign_detector.processing_thread, args=(self.image_queue, )) sign_worker.setDaemon(True) sign_worker.start() connection_worker = Thread(target=self.thread_refresh_connection) connection_worker.setDaemon(True) connection_worker.start() self.last_time_image_received = time.time() # Push init images to force model loading image_np = np.zeros([240, 320, 3], dtype=np.uint8) depth_image_np = np.zeros([240, 320], dtype=np.uint8) try: for _ in range(10): if self.image_queue.full(): break self.image_queue.put_nowait(image_np) for _ in range(10): if self.depth_image_queue.full(): break self.depth_image_queue.put_nowait(depth_image_np) except: pass
class ImageProcessor: def __init__(self, cv_bridge, debug_stream=None): ''' Init ros node to receive images ''' self.cv_bridge = cv_bridge self.debug_stream = debug_stream if self.debug_stream: self.debug_stream.create_stream('rgb', 'debug/rgb') self.debug_stream.create_stream('depth', 'debug/depth') # ================ Initialize controlling models ================ self.image_queue = Queue.Queue(maxsize=config.IMAGE_QUEUE_SIZE) self.depth_image_queue = Queue.Queue(maxsize=config.IMAGE_QUEUE_SIZE) # Segmentation seg_config = path.join(DATA_FOLDER, 'semantic_seg_ENet.conf.json') self.semantic_segmentation = SemanticSegmentation( seg_config, debug_stream=debug_stream) # Depth processor self.depth_processor = DepthProcessor(debug_stream=debug_stream) # Sign detector self.image_processor = SignDetector(cv_bridge) # Car controlling self.car_controller = CarController(self.semantic_segmentation, debug_stream=debug_stream) self.sign_detector = SignDetector(cv_bridge) # Setup pub/sub # WTF BUG!!! https://answers.ros.org/question/220502/image-subscriber-lag-despite-queue-1/ self.rgb_camera_sub = rospy.Subscriber( config.TOPIC_GET_IMAGE, CompressedImage, callback=self.callback_rgb_image, queue_size=1, buff_size=2**24) if config.USE_DEPTH_CAMERA: self.depth_camera_sub = rospy.Subscriber( config.TOPIC_GET_DEPTH_IMAGE, CompressedImage, callback=self.callback_depth_image, queue_size=1, buff_size=2**24) # Setup workers control_worker = Thread(target=self.thread_car_control, args=(self.image_queue, self.depth_image_queue)) control_worker.setDaemon(True) control_worker.start() sign_worker = Thread(target=self.sign_detector.processing_thread, args=(self.image_queue, )) sign_worker.setDaemon(True) sign_worker.start() connection_worker = Thread(target=self.thread_refresh_connection) connection_worker.setDaemon(True) connection_worker.start() self.last_time_image_received = time.time() # Push init images to force model loading image_np = np.zeros([240, 320, 3], dtype=np.uint8) depth_image_np = np.zeros([240, 320], dtype=np.uint8) try: for _ in range(10): if self.image_queue.full(): break self.image_queue.put_nowait(image_np) for _ in range(10): if self.depth_image_queue.full(): break self.depth_image_queue.put_nowait(depth_image_np) except: pass def thread_refresh_connection(self): ''' Monitor and restart image topic if we don't receive from image topic for a long time This prevent car freezing when start simulator after start controling node. This idea comes from https://github.com/RobotWebTools/rosbridge_suite/issues/298. ''' while True: time.sleep(3) if time.time() - self.last_time_image_received > 3: self.rgb_camera_sub.unregister() # WTF BUG!!! https://answers.ros.org/question/220502/image-subscriber-lag-despite-queue-1/ self.rgb_camera_sub = rospy.Subscriber( config.TOPIC_GET_IMAGE, CompressedImage, callback=self.callback_rgb_image, queue_size=1, buff_size=2**24) if config.USE_DEPTH_CAMERA: self.depth_camera_sub.unregister() self.depth_camera_sub = rospy.Subscriber( config.TOPIC_GET_DEPTH_IMAGE, CompressedImage, callback=self.callback_depth_image, queue_size=1, buff_size=2**24) def thread_car_control(self, image_queue, depth_image_queue): while True: img = image_queue.get() if config.USE_DEPTH_CAMERA: depth_img = depth_image_queue.get() self.car_controller.control(img, depth_img) else: self.car_controller.control(img) def callback_rgb_image(self, data): ''' Function to process rgb images ''' try: np_arr = np.fromstring(data.data, np.uint8) image_np = cv2.imdecode(np_arr, cv2.IMREAD_COLOR) # NOTE: image_np.shape = (240, 320, 3) image_np = cv2.resize(image_np, (320, 240)) for _ in range(2): if self.image_queue.full(): try: self.image_queue.get_nowait() except e: print(e) try: self.image_queue.put_nowait(image_np) except e: print(e) # self.car_controller.control(image_np) self.last_time_image_received = time.time() if self.debug_stream: self.debug_stream.update_image('rgb', image_np) except CvBridgeError as e: print(e) def callback_depth_image(self, data): try: np_arr = np.fromstring(data.data, np.uint8) image_np = cv2.imdecode(np_arr, cv2.IMREAD_GRAYSCALE) image_np = cv2.resize(image_np, (320, 240)) image = self.depth_processor.pre_processing_depth_img(image_np) for _ in range(2): if self.depth_image_queue.full(): try: self.depth_image_queue.get_nowait() except e: print(e) try: self.depth_image_queue.put_nowait(image) except e: print(e) except CvBridgeError as e: print(e)
class AutoDriftEnv(gym.Env): """ Custom gym environment for the Autodrift Grand Prix. For now, assume that camera image input is (80, 160, 3) (height, width, depth). Command history is not supported yet, and with it jerk penalty is unsupported. Frame skip is also not supported. """ metadata = {"render.modes": ["human", "rgb_array"]} def __init__(self, const_throttle=None): super(AutoDriftEnv, self).__init__() self.const_throttle = const_throttle # save the last n commands (throttle and steering) self.n_commands = 2 # interface to the car self.viewer = CarController() # action space if const_throttle is None: # define independent bounds for steering and throttle self.action_space = spaces.Box(low=np.array([-MAX_STEERING, -1]), high=np.array([MAX_STEERING, 1]), dtype=np.float32) else: # steering only self.action_space = spaces.Box(low=np.array([-MAX_STEERING]), high=np.array([MAX_STEERING]), dtype=np.float32) # use pixels as input self.observation_space = spaces.Box(low=0, high=255, shape=INPUT_DIM, dtype=np.uint8) def step(self, action): """ :param action: (np.ndarray) :return: (np.ndarray, float, bool, dict """ # action[0] = steering angle, action[1] = throttle if self.const_throttle is None: # variable throttle. convert throttle range -> [-1, 1] -> [0, 1] -> [MIN, MAX] temp = (action[1] + 1) / 2 action[1] = (1 - temp) * MIN_THROTTLE + temp * MAX_THROTTLE else: # fixed throttle action = np.concatenate([action, [self.const_throttle]]) # order the car self.viewer.take_action(action) observation, reward, done, info = self.viewer.observe() logger.debug("Reward: {0}, isDone: {1}".format(reward, done)) return observation, reward, done, info def reset(self): print("One env timestep done. Resetting after 0.5s...") # reset state of car controller, then sleep for 5s to replace car to start position manually self.viewer.reset() time.sleep(0.5) print("Now resuming...") observation, reward, done, info = self.viewer.observe() return observation def render(self, mode='rgb_array'): if mode == 'rgb_array': return self.viewer.image_array return None def close(self): # nothing actually happens here self.viewer.quit()
if len(sys.argv) > 1 and sys.argv[1] == '--debug': logging.info('debug mode') DEBUG = True else: logging.info('working mode') # start the camera streaming server camera = cam_streamer.CameraWrapper() camera_thread = threading.Thread(target=camera.run) camera_thread.start() logging.info("Cam streamer started") print("Cam streamer started") # start the socket server to receive commands cc = CarController() logging.info("Car controller started") print("Car controller started") while True: try: logging.info('opening socket') sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) sock.bind((HOST, PORT)) sock.listen(5) logging.info('listening on ' + HOST + ':' + str(PORT)) reset = False while reset == False: conn, addr = sock.accept() print('Connected by', addr) while True: data = conn.recv(1024)
class MainWindow(QMainWindow, Ui_MainWindow): # define signals signal_gesture_read = pyqtSignal(name="signal_gesture_read") signal_prediction_show = pyqtSignal(name="signal_prediction_show") signal_command_send = pyqtSignal(str, name='signal_command_send') def __init__(self, parent=None): super(MainWindow, self).__init__(parent) self.setupUi(self) # CNN self.model = load_model() if torch.cuda.is_available(): self.gpu = True self.model.cuda() self.prediction_frequency = 10 # each 10 images arise a prediction self.prediction_count = 0 # camera self.camera = cv2.VideoCapture() self.CAM_NUM = 0 self.camera_x0 = 400 self.camera_y0 = 200 self.camera_height = 200 self.camera_width = 200 # car video stream self.stream = CarStream() # car controller self.controller = None self.host_detector = HostDetector() # control self.timer_camera = QtCore.QTimer() self.timer_stream = QtCore.QTimer() self.horizontalSlider.setValue(70) self.threshold = self.horizontalSlider.value() # SLOT self.slot_init() @log_decorator def slot_init(self): self.timer_camera.timeout.connect(self.camera_show) self.pushButton_open_cam.clicked.connect(self.camera_open) self.pushButton_close_cam.clicked.connect(self.camera_close) self.pushButton_left.clicked.connect(self.box_left) self.pushButton_right.clicked.connect(self.box_right) self.pushButton_down.clicked.connect(self.box_down) self.pushButton_up.clicked.connect(self.box_up) self.pushButton_connect_car.clicked.connect(self.car_control_open) self.pushButton_disconnect.clicked.connect(self.car_control_close) self.horizontalSlider.valueChanged.connect(self.threshold_update) self.signal_prediction_show.connect(self.prediction_show) self.signal_gesture_read.connect(self.gesture_show) self.signal_command_send.connect(self.command_send) @log_decorator def car_control_open(self, *args): ''' open car camera and connect the car controller. :return: ''' print(">>> loading.") self.host_detector.detect_hosts() self.stream.pull() # call pull to init car camera. # check ESP 8266 connection if self.controller is None: controller_ip = None print("Found hosts:", self.host_detector.hosts) for host in self.host_detector.hosts: if 'esp' in host or 'ESP' in host: controller_ip = self.host_detector.hosts[host] break # TODO: seems ESP8266 can't be detect # used static IP for debug. # IP is pre-found from router configuration. if controller_ip is None: controller_ip = '192.168.43.213' # debug end. if controller_ip is not None: self.controller = CarController(controller_ip) self.controller.status = 'online' # TODO: check connection # self.controller.test() else: print(">>> ERROR: can't find ESP8266. Current hosts: ", self.host_detector.hosts) msg = QtWidgets.QMessageBox.warning( self, u"Warning", u"请检测汽车芯片ESP8266是否正确连接路由", buttons=QtWidgets.QMessageBox.Ok, defaultButton=QtWidgets.QMessageBox.Ok) else: # had connected before. self.controller.status = 'online' # open car video stream frame = self.stream.read_buffer() if frame is not None: if not self.timer_stream.isActive(): self.timer_stream.start(30) self.timer_stream.timeout.connect(self.stream_show) @log_decorator def car_control_close(self, *args): ''' close car camera stream and the connection of car controller. :return: ''' # disconnect with stream self.timer_stream.timeout.disconnect(self.stream_show) self.label_car.setText("Not recieve car video stream yet.") self.controller.status = 'idle' # @log_decorator def stream_show(self): ''' show camera image, emit signal_gesture_read signal :return: ''' frame = self.stream.read_buffer() frame = cv2.resize(frame, (640, 480)) show = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB) showImage = QtGui.QImage(show.data, show.shape[1], show.shape[0], QtGui.QImage.Format_RGB888) self.label_car.setPixmap(QtGui.QPixmap.fromImage(showImage)) @log_decorator def camera_open(self, *args): ''' open laptop camera :param args: :return: ''' if self.timer_camera.isActive() == False: flag = self.camera.open(self.CAM_NUM) if flag == False: msg = QtWidgets.QMessageBox.warning( self, u"Warning", u"请检测相机与电脑是否连接正确", buttons=QtWidgets.QMessageBox.Ok, defaultButton=QtWidgets.QMessageBox.Ok) else: if not self.timer_camera.isActive(): self.timer_camera.start(30) @log_decorator def camera_close(self, *args): ''' close laptop camera :param args: :return: ''' self.timer_camera.stop() self.camera.release() self.label_camera.clear() self.label_camera.setText("Not recieve car video stream yet.") def camera_show(self): ''' show camera image, emit signal_gesture_read signal :return: ''' flag, frame = self.camera.read() frame = cv2.flip(frame, 3) frame = cv2.resize(frame, (640, 480)) self.image = frame # notations threshold_str = "Current threshlold: %d %%" % self.threshold cv2.putText(frame, 'Driver Romm <( ^-^ )>', (FX, FY), FONT, SIZE, (0, 255, 0), 1, 1) cv2.putText(frame, threshold_str, (FX, FY + 2 * FH), FONT, SIZE, (0, 255, 0), 1, 1) cv2.rectangle(frame, (self.camera_x0, self.camera_y0), (self.camera_x0 + self.camera_width, self.camera_y0 + self.camera_height), (0, 255, 0), 1) show = cv2.cvtColor(self.image, cv2.COLOR_BGR2RGB) showImage = QtGui.QImage(show.data, show.shape[1], show.shape[0], QtGui.QImage.Format_RGB888) self.label_camera.setPixmap(QtGui.QPixmap.fromImage(showImage)) # call gesture filter self.signal_gesture_read.emit() # @log_decorator def gesture_show(self): ''' show gesture image after binary mask, emit signal_prediction_show signal :return: ''' roi = skinMask(self.image, self.camera_x0, self.camera_y0, self.camera_width, self.camera_height) showImage = QtGui.QImage(roi.data, roi.shape[1], roi.shape[0], QtGui.QImage.Format_Grayscale8) self.label_gesture.setPixmap(QtGui.QPixmap.fromImage(showImage)) # predict image if self.prediction_count == self.prediction_frequency: t = threading.Thread(target=models.predict_gesture, args=[self.model, roi], kwargs={"verbose": False}) t.start() self.prediction_count = 0 else: self.prediction_count += 1 self.signal_prediction_show.emit() # @log_decorator def prediction_show(self): ''' show prediction, and emit signal with prediction to controller :return: ''' # draw probability plot plot = models.update() cmd = None # write result if len(global_vars.jsonarray) > 0: prediction = max(global_vars.jsonarray, key=lambda x: global_vars.jsonarray[x]) if global_vars.jsonarray[prediction] * 100 > self.threshold: self.label_predict.setText(GESTURE_CLASSES[int(prediction)]) cmd = GESTURE_CLASSES[int(prediction)] else: self.label_predict.setText('nothing') showImage = QtGui.QImage(plot.data, plot.shape[1], plot.shape[0], QtGui.QImage.Format_RGB888) self.label_probability.setPixmap(QtGui.QPixmap.fromImage(showImage)) # emit sigal if cmd is not None and self.controller.status == 'online': self.signal_command_send.emit(cmd) @log_decorator def keyPressEvent(self, event): ''' rewrite keyboard event for car controller debug. :param event: :return: ''' if self.controller is not None and self.controller.status == 'online': if event.key() == Qt.Key_W: # send move forward signal self.signal_command_send.emit("go_ahead") elif event.key() == Qt.Key_A: # send turn left signal self.signal_command_send.emit("turn_left") elif event.key() == Qt.Key_D: # send turn right signal self.signal_command_send.emit("turn_right") elif event.key() == Qt.Key_S: # send back up signal: self.signal_command_send.emit("back_up") else: print("Not connect to ESP8266 yet.") if event.key() == Qt.Key_I: # try to find ESP 8266 's IP self.host_detector.detect_hosts() elif event.key() == Qt.Key_P: # print current detected IPs print(self.host_detector.hosts) # SLOT: update value def box_left(self): self.camera_x0 -= 5 def box_right(self): self.camera_x0 += 5 def box_up(self): self.camera_y0 -= 5 def box_down(self): self.camera_y0 += 5 def threshold_update(self): self.threshold = self.horizontalSlider.value() def command_send(self, prediction): self.controller.move(prediction)
from threading import Thread from car_controller import CarController import api if __name__ == '__main__': try: controller = CarController() thread: Thread = Thread(target=lambda: api.init(controller)) thread.setDaemon(True) thread.start() controller.follow_waypoints() except InterruptedError as e: # Shutdown on ctrl+c or window closed print('Shutting down as per user request...') exit(0)