pavel/kick-it
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

improvements in color calibration and tracking

Browse Source
This commit is contained in:
Pavel Lutskov
2018-06-02 12:10:42 +02:00
parent 51d2a139f9
commit a9029061a0
7 changed files with 104 additions and 59 deletions
Download Patch File Download Diff File Expand all files Collapse all files

80
scripts/live_recognition_with_head.py
View File

@@ -1,13 +1,11 @@
from __future__ import print_function
from __future__ import division
import cv2
import numpy as np
#import imutils
from naoqi import ALProxy
from collections import deque
from imagereaders import NaoImageReader
from live_recognition import BallFinder
from live_recognition_with_head_with_body import move_to
# Nao configuration
@@ -28,8 +26,7 @@ min_radius = 5
resized_width = None # Maybe we need it maybe don't (None if don't)
current_value = 0
global counter
counter=0
counter = 0
def get_angle():
robotIP="192.168.0.11"
@@ -46,50 +43,57 @@ def set_angle_new(x,y):
robotIP="192.168.0.11"
PORT = 9559
motionProxy = ALProxy("ALMotion", robotIP, PORT)
# activiert gelenke
motionProxy.setStiffnesses("Head", 0.5)
names = ["HeadYaw", "HeadPitch"]
fractionMaxSpeed = 0.025
x_mid=320/2
y_mid=240/2
#y_mid=120
x_diff=x-x_mid
y_diff=y-y_mid
fractionMaxSpeed = 0.3
x_diff=x-0.5
y_diff=y-0.5
print("x_diff="+str(x_diff))
print("y_diff="+str(y_diff))
#videoProxy=ALProxy('ALVideoDevice', robotIP, PORT)
#ball_angles=videoProxy.getAngularPositionFromImagePosition(cam_id,[x/320,y/240])
#print(ball_angles)
#ball_angle_x=ball_angles[0]
#ball_angle_y=ball_angles[1]
#print("ball_angle_x="+str(ball_angle_x))
#print("ball_angle_y="+str(ball_angle_y))
#ball_angle_x_diff=ball_angle_x+
#ball_angle_y_diff=ball_angle_y-99
#print(ball_angle_x_diff*3.14/180)
#print(ball_angle_y_diff)
# print(ball_angles-[-169,99])
#[-169.53343200683594, 99.27782440185547] (x_mid,y_mid)
#if abs(ball_angle_x)>0.2 and abs(ball_angle_y)>0.01:
#angles=[ball_angle_x,0]
#angles=[0.25*ball_angle_x,0.25*ball_angle_y]
#angles=[0.25*ball_angle_x,0.25*ball_angle_y]
#angles=[2*(-1 if x_diff > 0 else 1),2*(-1 if y_diff > 0 else 1)]
angles=[2*(-1 if x_diff > 0 else 1),0]
# videoProxy=ALProxy('ALVideoDevice', robotIP, PORT)
# ball_angles=videoProxy.getAngularPositionFromImagePosition(cam_id,[x/320,y/240])
# print(ball_angles)
# ball_angle_x=ball_angles[0]
# ball_angle_y=ball_angles[1]
# print("ball_angle_x="+str(ball_angle_x))
# print("ball_angle_y="+str(ball_angle_y))
# ball_angle_x_diff=ball_angle_x+
# ball_angle_y_diff=ball_angle_y-99
# print(ball_angle_x_diff*3.14/180)
# print(ball_angle_y_diff)
# print(ball_angles-[-169,99])
# [-169.53343200683594, 99.27782440185547] (x_mid,y_mid)
# if abs(ball_angle_x)>0.2 and abs(ball_angle_y)>0.01:
# angles=[ball_angle_x,0]
# angles=[0.25*ball_angle_x,0.25*ball_angle_y]
# angles=[0.25*ball_angle_x,0.25*ball_angle_y]
# angles=[2*(-1 if x_diff > 0 else 1),2*(-1 if y_diff > 0 else 1)]
angles=[-x_diff / 2, 0]
#if abs(ball_angle_x)>0.1 or abs(ball_angle_y)>0.1:
#if abs(x_diff)>50 or abs(y_diff)>50:
if abs(x_diff)>50:
motionProxy.changeAngles(names, angles, fractionMaxSpeed)
global counter
if abs(x_diff) > 0.1:
motionProxy.changeAngles(names, angles, fractionMaxSpeed)
counter = 0
else:
counter += 1
print(counter)
if counter == 10:
print('Going to rotate')
angle = get_angle()
if abs(angle[1]) > 0.1:
move_to(0, angle[1])
#motionProxy.setAngles(names,angles,fractionMaxSpeed)
#else:
# a=get_angle()
# motionProxy.setAngles(names,a,fractionMaxSpeed)
'''
elif abs(ball_angle_x)<0.1 and abs(ball_angle_y)<0.1:
#tts = ALProxy("ALTextToSpeech", "192.168.0.11", 9559)
@@ -160,12 +164,16 @@ if __name__ == '__main__':
video = NaoImageReader(nao_ip, port=nao_port, cam_id=cam_id, res=res,
fps=fps)
finder = BallFinder(red_lower, red_upper, 5, None)
finder.load_hsv_config('ball_hsv.json')
try:
while True:
try:
(x, y), radius = finder.find_colored_ball(video.get_frame())
except TypeError:
continue
print(x, y)
x, y = video.to_relative(x, y)
print(x, y)
set_angle_new(x,y)
'''
if 0<y<100: