From 06bcf565f30ce8b68ecf1a5eafbe12932bee6592 Mon Sep 17 00:00:00 2001 From: spirkelmann Date: Mon, 6 May 2019 22:23:31 +0200 Subject: [PATCH] added position detection with opencv --- remote_control/keyboard_controller.py | 39 ++++++ remote_control/position_controller.py | 169 ++++++++++++++++++++++++++ 2 files changed, 208 insertions(+) create mode 100644 remote_control/keyboard_controller.py create mode 100644 remote_control/position_controller.py diff --git a/remote_control/keyboard_controller.py b/remote_control/keyboard_controller.py new file mode 100644 index 0000000..80bbac9 --- /dev/null +++ b/remote_control/keyboard_controller.py @@ -0,0 +1,39 @@ +import socket +import pygame + +pygame.init() +pygame.display.set_mode((640, 480)) + +rc_socket = socket.socket() +try: + rc_socket.connect(('192.168.4.1', 1234)) # connect to robot +except socket.error: + print("could not connect to socket") + + +while True: + events = pygame.event.get() + for event in events: + if event.type == pygame.KEYDOWN: + if event.key == pygame.K_LEFT: + u1 = 1.0 + u2 = -1.0 + print("turn left: ({},{})".format(u1, u2)) + elif event.key == pygame.K_RIGHT: + u1 = -1.0 + u2 = 1.0 + print("turn right: ({},{})".format(u1, u2)) + elif event.key == pygame.K_UP: + u1 = -1.0 + u2 = -1.0 + print("forward: ({},{})".format(u1, u2)) + elif event.key == pygame.K_DOWN: + u1 = 1.0 + u2 = 1.0 + print("forward: ({},{})".format(u1, u2)) + rc_socket.send('({},{})\n'.format(u1, u2)) + elif event.type == pygame.KEYUP: + u1 = 0 + u2 = 0 + print("key released, resetting: ({},{})".format(u1, u2)) + rc_socket.send('({},{})\n'.format(u1, u2)) diff --git a/remote_control/position_controller.py b/remote_control/position_controller.py new file mode 100644 index 0000000..a01be87 --- /dev/null +++ b/remote_control/position_controller.py @@ -0,0 +1,169 @@ +# startup: +# roscore +# rosrun cv_camera cv_camera_node + +import sys +import rospy +import pygame +import numpy as np +import cv2 +import cv2.aruco as aruco +import socket + +from sensor_msgs.msg import Image +from sensor_msgs.msg import CompressedImage +from cv_bridge import CvBridge, CvBridgeError + +pygame.init() +pygame.font.init() +#pygame.joystick.init() + +myfont = pygame.font.SysFont('Comic Sans MS', 30) +pygame.display.set_caption("ROS camera stream on Pygame") +screenheight = 480 +screenwidth = 640 #4*screenheight//3 +screen = pygame.display.set_mode([screenwidth, screenheight]) +red = (255, 0, 0) +teal = (0, 255, 255) + +# ros setup +camera_stream = "/cv_camera/image_raw" +#camera_stream = "/image_raw" +compression = False + +class Robot: + def __init__(self, id, ip=None): + self.pos = None + + self.id = id + + self.ip = None + +class RemoteController: + def __init__(self): + self.cam = cv2.VideoCapture(2) + + #self.image_pub = rospy.Publisher("pygame_image", Image) + + #self.bridge = CvBridge() + + #if compression: + # self.image_sub = rospy.Subscriber(camera_stream + "/compressed", CompressedImage, self.callback) + #else: + # self.image_sub = rospy.Subscriber(camera_stream, Image, self.callback) + + #self.cv_image = np.zeros((1, 1, 3), np.uint8) + + self.robots = [Robot(1), Robot(2), Robot(512)] + + screen.fill([0, 0, 0]) + + self.aruco_dict = aruco.Dictionary_get(aruco.DICT_ARUCO_ORIGINAL) + self.parameters = aruco.DetectorParameters_create() + + self.rc_socket = socket.socket() + try: + pass + self.rc_socket.connect(('192.168.4.1', 1234)) # connect to robot + except socket.error: + print("could not connect to socket") + + def callback(self, data): + try: + if compression: + self.cv_image = self.bridge.compressed_imgmsg_to_cv2(data) + else: + self.cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8") + except CvBridgeError as e: + print(e) + #self.cv_image_small = np.fliplr(self.cv_image_small) # why is this necessary? + + # marker detection + #gray = cv2.cvtColor(self.cv_image, cv2.COLOR_BGR2GRAY) + + + #print("robot {} pos = {}".format(r.id, r.pos)) + + def capture(self): + ret_val, self.cv_image = self.cam.read() + + corners, ids, rejectedImgPoints = aruco.detectMarkers(self.cv_image, self.aruco_dict, parameters=self.parameters) + rvec, tvec, _ = aruco.estimatePoseSingleMarkers(corners[0], 0.05, mtx, dist) + marker_found = len(corners) > 0 + + if marker_found: + for i in range(len(corners)): + #print("id = {}".format(ids[i])) + pos = np.mean(corners[i][0], axis=0) + + for r in self.robots: + if r.id == ids[i]: + pos_flipped = (self.cv_image.shape[1] - int(pos[0]), int(pos[1])) + r.pos = pos_flipped + + def show_display(self): + while True: + self.capture() + + # show ros camera image on the pygame screen + #image = cv2.resize(self.cv_image,(screenwidth,screenheight)) + frame = cv2.cvtColor(self.cv_image, cv2.COLOR_BGR2RGB) + frame = np.rot90(frame) + frame = pygame.surfarray.make_surface(frame) + screen.blit(frame, (0, 0)) + + # plot robot positions + for r in self.robots: + if r.pos is not None: + #print("drawing at {}".format(r.pos)) + pygame.draw.circle(screen, (255, 0, 0), r.pos, 10) + + pygame.display.update() + + events = pygame.event.get() + for event in events: + if event.type == pygame.KEYDOWN: + if event.key == pygame.K_LEFT: + u1 = 1.0 + u2 = -1.0 + #print("turn left: ({},{})".format(u1, u2)) + elif event.key == pygame.K_RIGHT: + u1 = -1.0 + u2 = 1.0 + #print("turn right: ({},{})".format(u1, u2)) + elif event.key == pygame.K_UP: + u1 = -1.0 + u2 = -1.0 + #print("forward: ({},{})".format(u1, u2)) + elif event.key == pygame.K_DOWN: + u1 = 1.0 + u2 = 1.0 + #print("forward: ({},{})".format(u1, u2)) + self.rc_socket.send('({},{})\n'.format(u1, u2)) + elif event.type == pygame.KEYUP: + u1 = 0 + u2 = 0 + #print("key released, resetting: ({},{})".format(u1, u2)) + self.rc_socket.send('({},{})\n'.format(u1, u2)) + + +def main(args): + rospy.init_node('controller_node', anonymous=True) + + rc = RemoteController() + + pygame.init() + pygame.display.set_mode((640, 480)) + + rc.show_display() + #game.loop() + + # try: + # rospy.spin() + # except KeyboardInterrupt: + # print("Shutting down") + # cv2.destroyAllWindows() + + +if __name__ == '__main__': + main(sys.argv) \ No newline at end of file