def traj_dir_toward_line(self, line): """ @brief Determines if the current trajectory is moving toward the line Takes the most and least recent points, and determines if the overall direction of the trajectory is generally toward the given Line or away. This is done by comparing the distance between the current point and oldest point. In context of the robot goalie, will determine if the robot is currently moving towards the robot axis, irrespective of bounces. Used to tell if the ball is generally moving towards or away from the axis if dist(curr_pt to line) < dist(old_pt to line): return 1 else return 0 @param line The Line object used @return 1 if moving towards line, 0 if not """ if None in self.pt_list or line is None: return 0 curr_pt = self.pt_list[self.curr_index] last_pt = self.pt_list[self.last_index] unused, distances = utils.distance_from_line( [curr_pt, last_pt], line, squared=1) # current point is closer to line than last point if distances[0] < distances[1]: return 1 return 0
def stream(tracker, camera=0, server=0): """ @brief Captures video and runs tracking and moves robot accordingly @param tracker The BallTracker object to be used @param camera The camera number (0 is default) for getting frame data camera=1 is generally the first webcam plugged in """ ######## GENERAL PARAMETER SETUP ######## MOVE_DIST_THRESH = 20 # distance at which robot will stop moving SOL_DIST_THRESH = 150 # distance at which solenoid fires PACKET_DELAY = 1 # number of frames between sending data packets to pi OBJECT_RADIUS = 13 # opencv radius for circle detection AXIS_SAFETY_PERCENT = 0.05 # robot stops if within this % dist of axis edges packet_cnt = 0 tracker.radius = OBJECT_RADIUS ######## SERVER SETUP ######## motorcontroller_setup = False if server: # Create a TCP/IP socket sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) # Obtain server address by going to network settings and getting eth ip server_address = ('169.254.171.10',10000) # CHANGE THIS #server_address = ('localhost', 10000) # for local testing print 'starting up on %s port %s' % server_address sock.bind(server_address) sock.listen(1) connection, client_address = None, None while True: # Wait for a connection print 'waiting for a connection' connection, client_address = sock.accept() break ######## CV SETUP ######## # create video capture object for #cap = cv2.VideoCapture(camera) cap = WebcamVideoStream(camera).start() # WEBCAM #cap = cv2.VideoCapture('../media/goalie-test.mov') #cap = cv2.VideoCapture('../media/bounce.mp4') cv2.namedWindow(tracker.window_name) # create trajectory planner object # value of bounce determines # of bounces. 0 is default (no bounces) # bounce not currently working correctly planner = TrajectoryPlanner(frames=4, bounce=0) # create FPS object for frame rate tracking fps_timer = FPS(num_frames=20) while(True): # start fps timer fps_timer.start_iteration() ######## CAPTURE AND PROCESS FRAME ######## ret, frame = True, cap.read() # WEBCAM #ret, frame = cap.read() # for non-webcam testing if ret is False: print 'Frame not read' exit() # resize to 640x480, flip and blur frame,img_hsv = tracker.setup_frame(frame=frame, w=640,h=480, scale=1, blur_window=15) ######## TRACK OBJECTS ######## # use the HSV image to get Circle objects for robot and objects. # object_list is list of Circle objects found. # robot is single Circle for the robot position # robot_markers is 2-elem list of Circle objects for robot markers object_list = tracker.find_circles(img_hsv.copy(), tracker.track_colors, tracker.num_objects) robot, robot_markers = tracker.find_robot_system(img_hsv) walls = tracker.get_rails(img_hsv, robot_markers, colors.Yellow) planner.walls = walls # Get the line/distances between the robot markers # robot_axis is Line object between the robot axis markers # points is list of Point objects of closest intersection w/ robot axis # distanes is a list of distances of each point to the robot axis robot_axis = utils.line_between_circles(robot_markers) points, distances = utils.distance_from_line(object_list, robot_axis) planner.robot_axis = robot_axis ######## TRAJECTORY PLANNING ######## # get closest object and associated point, generate trajectory closest_obj_index = utils.min_index(distances) # index of min value closest_line = None closest_pt = None if closest_obj_index is not None: closest_obj = object_list[closest_obj_index] closest_pt = points[closest_obj_index] closest_line = utils.get_line(closest_obj, closest_pt) # only for viewing planner.add_point(closest_obj) # Get trajectory - list of elements for bounces, and final line traj # Last line intersects with robot axis traj_list = planner.get_trajectory_list(colors.Cyan) traj = planner.traj ######## SEND DATA TO CLIENT ######## if packet_cnt != PACKET_DELAY: packet_cnt = packet_cnt + 1 else: packet_cnt = 0 # reset packet counter # error checking to ensure will run properly if len(robot_markers) is not 2 or robot is None or closest_pt is None: pass elif server: try: if motorcontroller_setup is False: # send S packet for motorcontroller setup motorcontroller_setup = True ######## SETUP MOTORCONTROLLER ######## axis_pt1 = robot_markers[0].to_pt_string() axis_pt2 = robot_markers[1].to_pt_string() data = 'SM '+axis_pt1+' '+axis_pt2+' '+robot.to_pt_string() print data connection.sendall(data) # setup is done, send packet with movement data else: obj_robot_dist = utils.get_pt2pt_dist(robot, closest_obj) print 'dist: ' + str(obj_robot_dist) # USE FOR CALIBRATION ######## SOLENOID ACTIVATION CODE ######## # check if solenoid should fire if obj_robot_dist <= SOL_DIST_THRESH: # fire solenoid, dont move print 'activate solenoid!' # TODO SEND SOLENOID ACTIVATE ######## MOTOR CONTROL ######## # get safety parameters rob_ax1_dist = utils.get_pt2pt_dist(robot_markers[0],robot) rob_ax2_dist = utils.get_pt2pt_dist(robot_markers[1],robot) axis_length = utils.get_pt2pt_dist(robot_markers[0], robot_markers[1]) # ensure within safe bounds of motion relative to axis # if rob_ax1_dist/axis_length <= AXIS_SAFETY_PERCENT or \ # rob_ax2_dist/axis_length <= AXIS_SAFETY_PERCENT: # # in danger zone, kill motor movement # print 'INVALID ROBOT LOCATION: stopping motor' # data = 'KM' # connection.sendall(data) # if in danger zone near axis edge, move towards other edge if rob_ax1_dist/axis_length <= AXIS_SAFETY_PERCENT: print 'INVALID ROBOT LOCATION' data = 'MM '+robot.to_pt_string()+' '+robot_markers[1].to_pt_string() elif rob_ax2_dist/axis_length <= AXIS_SAFETY_PERCENT: print 'INVALID ROBOT LOCATION' data = 'MM '+robot.to_pt_string()+' '+robot_markers[0].to_pt_string() # check if robot should stop moving elif obj_robot_dist <= MOVE_DIST_THRESH: # obj close to robot # Send stop command, obj is close enough to motor to hit data = 'KM' print data connection.sendall(data) pass # Movement code else: # far enough so robot should move #### FOR TRAJECTORY ESTIMATION # if planner.traj is not None: # axis_intersect=shapes.Point(planner.traj.x2,planner.traj.y2) # # Clamp the point to send to the robot axis # traj_axis_pt = utils.clamp_point_to_line( # axis_intersect, robot_axis) # data = 'D '+robot.to_pt_string()+' '+traj_axis_pt.to_string() # connection.sendall(data) #### FOR CLOSEST POINT ON AXIS #### if closest_pt is not None and robot is not None: # if try to move more than length of axis, stop instead if utils.get_pt2pt_dist(robot,closest_pt) > axis_length: print 'TRYING TO MOVE OUT OF RANGE' data = 'KM' print data connection.sendall(data) else: data = 'MM ' + robot.to_pt_string() + ' ' + \ closest_pt.to_string() print data connection.sendall(data) except IOError: pass # don't send anything ######## ANNOTATE FRAME FOR VISUALIZATION ######## frame = gfx.draw_lines(img=frame, line_list=walls) frame = gfx.draw_robot_axis(img=frame, line=robot_axis) # draw axis line frame = gfx.draw_robot(frame, robot) # draw robot frame = gfx.draw_robot_markers(frame, robot_markers) # draw markers frame = gfx.draw_circles(frame, object_list) # draw objects # eventually won't need to print this one frame = gfx.draw_line(img=frame, line=closest_line) # closest obj>axis # draw full set of trajectories, including bounces #frame = gfx.draw_lines(img=frame, line_list=traj_list) #frame = gfx.draw_line(img=frame, line=traj) # for no bounces frame = gfx.draw_point(img=frame, pt=closest_pt) #frame=gfx.draw_line(frame,planner.debug_line) # for debug ######## FPS COUNTER ######## fps_timer.get_fps() fps_timer.display(frame) ######## DISPLAY FRAME ON SCREEN ######## cv2.imshow(tracker.window_name,frame) # quit by pressing q if cv2.waitKey(1) & 0xFF == ord('q'): break # release capture cap.stop() # WEBCAM #cap.release() # for testing w/o webcam cv2.destroyAllWindows()