added position detection with opencv
parent
de67a163d6
commit
06bcf565f3
|
|
@ -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))
|
||||
|
|
@ -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)
|
||||
Loading…
Reference in New Issue