diff --git a/pykick/colorpicker.py b/pykick/colorpicker.py index 7b40da6..543d2be 100644 --- a/pykick/colorpicker.py +++ b/pykick/colorpicker.py @@ -1,3 +1,6 @@ +"A script for calibrating the color and saving parameters." + + from __future__ import print_function from __future__ import division @@ -16,6 +19,15 @@ class Colorpicker(object): WINDOW_DETECTION_NAME = 'Primary Mask' def __init__(self, target=None): + """ + Parameters + ---------- + target : string + 'ball', 'goal' or 'field', affects detection algorithm, what config + to read and what config to save + + """ + parameters = ['low_h', 'low_s', 'low_v', 'high_h', 'high_s', 'high_v'] maxes = [180, 255, 255, 180, 255, 255] checkers = [ @@ -66,14 +78,17 @@ class Colorpicker(object): ] def do_print(self): + """Print the current calibration values.""" print(tuple(map(self.settings.get, ('low_h', 'low_s', 'low_v'))), tuple(map(self.settings.get, ('high_h', 'high_s', 'high_v')))) def _on_trackbar(self, val, name, checker): + """Update GUI.""" self.settings[name] = checker(val) self._hsv_updated(name) def _hsv_updated(self, param): + """Update GUI.""" cv2.setTrackbarPos(param, self.WINDOW_DETECTION_NAME, self.settings[param]) if self.marker is None: @@ -86,6 +101,7 @@ class Colorpicker(object): ) def show_frame(self, frame, width=None, manual=False): + """Show next frame.""" frame = imresize(frame, width=width) if self.marker is not None: thr = self.marker.primary_mask(frame) @@ -103,14 +119,23 @@ class Colorpicker(object): cv2.imshow(self.WINDOW_DETECTION_NAME, thr) return cv2.waitKey(0 if manual else 1) - def save(self, filename, color): + def save(self, filename, target): + """Save the config. + + Parameters + ---------- + filename : string + target : string + 'ball', 'goal' or 'field' + + """ try: with open(filename) as f: conf = json.load(f) except IOError: conf = {} conf.update( - {color: [ + {target: [ list(map(self.settings.get, ['low_h', 'low_s', 'low_v'])), list(map(self.settings.get, ['high_h', 'high_s', 'high_v'])) ]} @@ -118,19 +143,27 @@ class Colorpicker(object): with open(filename, 'w') as f: json.dump(conf, f, indent=4) - def load(self, filename, color): + def load(self, filename, target): + """Load the config. + + Parameters + ---------- + filename : string + target : string + 'ball', 'goal' or 'field' + + """ with open(filename) as f: jdict = json.load(f) self.settings = dict( zip(['low_h', 'low_s', 'low_v', 'high_h', 'high_s', 'high_v'], - jdict[color][0] + jdict[color][1]) + jdict[target][0] + jdict[target][1]) ) for name in self.settings: self._hsv_updated(name) if __name__ == '__main__': - defaults = read_config() parser = argparse.ArgumentParser( epilog='When called without arguments specifying the video source, ' + @@ -204,17 +237,17 @@ if __name__ == '__main__': cp.load(args.input_config, args.target) with InterruptDelayed(): - if args.video_file: + if args.video_file: # read from video rdr = VideoReader(args.video_file, loop=True) - elif args.image_file: + elif args.image_file: # read from picture rdr = PictureReader(args.image_file) - elif args.nao_ip: + elif args.nao_ip: # read from nao rdr = NaoImageReader( args.nao_ip, cam_id=args.nao_cam, res=args.nao_res ) - else: + else: # read from webcam rdr = VideoReader(0) try: diff --git a/pykick/striker.py b/pykick/striker.py index 2571f2c..6e5e3d2 100644 --- a/pykick/striker.py +++ b/pykick/striker.py @@ -1,3 +1,5 @@ +"""The class implementing high-level striker behavior.""" + from __future__ import print_function from __future__ import division @@ -16,6 +18,31 @@ class Striker(object): def __init__(self, nao_ip, nao_port, res, ball_hsv, goal_hsv, field_hsv, ball_min_radius, do_capture=False): + """For an example initialization see `__main__.py` + + Parameters + ---------- + nao_ip : string + Address of the NAO robot. + nao_port : int + TCP port on which the AIProxy listens. + res : int + Nao camera resolution. 0, 1, 2 or 3, see `imagereaders.py` for + res description. + ball_hsv : list + Ball HSV parameters in format [[LH, LS, LV], [HH, HS, HV]] + goal_hsv : list + Goal HSV parameters in format [[LH, LS, LV], [HH, HS, HV]] + field_hsv : list + Field HSV parameters in format [[LH, LS, LV], [HH, HS, HV]] + ball_min_radius : float + Minimal radius of the ball, as a fraction of the frame height + (for example 0.01 is 1% of the frame height) + do_capture : bool + Record the POV video or not (don't use, it's gonna crash the + robot) + + """ # Maintenance self.run_id = strftime('%Y%m%d%H%M%S') @@ -68,6 +95,7 @@ class Striker(object): self._timer_stop = 0 def close(self): + """Finish everyting and rest.""" self.is_over = True if self.tts_thread.isAlive(): @@ -82,6 +110,7 @@ class Striker(object): self.mover.stop_moving() def _speaker(self): + """Internal method: don't use.""" while not self.is_over: while self.speach_queue: text = self.speach_queue.pop() @@ -95,6 +124,7 @@ class Striker(object): sleep(0.5) def _pov(self): + """Don't use.""" while not self.is_over: try: self.upper_pov.get_frame() @@ -105,6 +135,7 @@ class Striker(object): continue def speak(self, text): + """Say stuff (useful for debugging).""" if not self.speach_history or self.speach_history[-1] != text: self.speach_queue.appendleft(text) self.speach_history.append(text) @@ -118,8 +149,8 @@ class Striker(object): # determine direction of head rotation sign = 1 if yaw >= 0 else -1 - # the robot starts to move arround his z-Axis in the direction where his - # head is aligned when the head yaw angle has reached his maximum + # the robot starts to move arround his z-Axis in the direction where + # his head is aligned when the head yaw angle has reached his maximum if yaw > pi/3: self.mover.set_head_angles(-pi / 8, pitch, 0.5) sleep(0.5) @@ -133,7 +164,22 @@ class Striker(object): sleep(0.3) def get_ball_angles_from_camera(self, cam, mask=True): - """Detect the ball and return its angles in camera coordinates.""" + """Detect the ball and return its angles in camera coordinates. + + Parameters + ---------- + cam : NaoImageReader + A camera object form which to read + mask : bool + Mask out the field or not + + Returns + ------- + tuple or None + (x, y) coordinates in camera angles in radians or None if ball + not found + + """ try: frame = cam.get_frame() except RuntimeError as e: # Sometimes camera doesn't return an image @@ -157,6 +203,22 @@ class Striker(object): return x, y def get_goal_angles_from_camera(self, cam, mask=True): + """Get the goal left post, center and right post. + + Parameters + ---------- + cam : NaoImageReader + A camera object from which to get frames. + mask : bool + Mask out the field or not. + + Returns + ------- + tuple or None + (left, right, center) camera angles of goal in radians or None + if not found + + """ try: frame = cam.get_frame() except RuntimeError as e: # Sometimes camera doesn't return an image @@ -183,6 +245,14 @@ class Striker(object): return goal_l, goal_r, goal_c def distance_to_ball(self): + """Measure the distance to the ball in meters. + + Raises + ------ + ValueError + The ball could not be seen :( + + """ camera = 'upper' angles = self.get_ball_angles_from_camera(self.upper_camera) if angles == None: @@ -202,7 +272,16 @@ class Striker(object): return (asin(0.40 / d) if hypo == 'bdist' else atan(0.2 / d)) * lr def ball_tracking(self, soll=0, tol=0.15): - """Track the ball using the feed from top and bottom camera. + """Turn the robot to the ball, find the ball if cannot see it. + + Parameters + ---------- + soll : float + X-Angle in radians, under which the ball should appear in the + robot's camera (maybe we want the robot not to look at the ball + directly). + tol : float + Allowable deviation in radians from soll-angle. Returns ------- @@ -245,16 +324,29 @@ class Striker(object): print() def run_to_ball(self, d): + """Fancy name for GO FORWARD.""" self.mover.move_to(d, 0, 0) # self.mover.wait() def turn_to_ball(self, ball_x, ball_y, tol=0.15, soll=0): - """Align robot to the ball. + """Align robot to the ball, assuming the ball is visible. + + This method is mainly used by `turn_to_ball`. If head is not centered at the ball (within tolerance), then turn head to ball. If after that the angle of head to body becomes too big, rotate the body by the head angle and simultaneously rotate head into 0 position to achieve alignment. + + Parameters + ---------- + soll : float + X-Angle in radians, under which the ball should appear in the + robot's camera (maybe we want the robot not to look at the ball + directly). + tol : float + Allowable deviation in radians from soll-angle. + """ # only the x ball angle is relevant for alignment @@ -287,6 +379,15 @@ class Striker(object): return True def align_to_ball(self): + """Perform one step of aligning behind the ball for the kick. + + Call this until this returns True. + + Returns + ------- + bool + True if nicely aligned else False. + """ ball_angles = self.get_ball_angles_from_camera(self.lower_camera, mask=False) if ball_angles is None: @@ -305,6 +406,15 @@ class Striker(object): return False def align_to_goal(self): + """Perform one step of aligning ball and goal. + + Call this until this returns True. + + Returns + ------- + bool + True if nicely aligned else False. + """ ball_angles = self.get_ball_angles_from_camera(self.lower_camera, mask=False) if ball_angles is None: @@ -351,6 +461,13 @@ class Striker(object): return False def goal_search(self): + """Find the goal. Return the angles in the robot frame. + + Returns + ------- + The same as the `get_goal_angles_from_camera`. + + """ self.speak('Searching for goal') print('Last goal:', self.last_goal) goal_angles = None