def startPlanner(d_goal=0.0, th_goal=0.0, x_goal=0.0, y_goal=0.0, th_goal_abs=0.0, goaltype="None"): """ Send a goal message to topic 'goal' """ """ Draw court """ global Goal_x, Goal_y graphics.canvas.delete(ALL) graphics.draw_court() if (goaltype == "newball"): th_goal = Ballbot_theta - math.radians( th_goal) # angle to goal in global coord # = angle of car in global coord - angle to goal from car (which is in [-90deg,90deg] x2 = Ballbot_x * 100.0 + d_goal * math.cos(th_goal) y2 = Ballbot_y * 100.0 + d_goal * math.sin(th_goal) th2 = 0 # doesn't matter for goal test # publish goal goal_msg = Goal() goal_msg.goaltype = String("newball") goal_msg.pose = Pose(x2, y2, th2) graphics.draw_point(x2, y2, th2, color='red') graphics.canvas.update_idletasks() pub_goal.publish(goal_msg) print "sent goal" Goal_x = x2 Goal_y = y2 elif (goaltype == "gotopose"): x2 = x_goal y2 = y_goal th2 = math.radians(th_goal_abs) # publish goal goal_msg = Goal() goal_msg.goaltype = String("gotopose") goal_msg.pose = Pose(x2, y2, th2) graphics.draw_point(x2, y2, th2, color='red') graphics.canvas.update_idletasks() pub_goal.publish(goal_msg) print "sent goal" Goal_x = x2 Goal_y = y2 """
def startPlanner(d_goal=0.0,th_goal=0.0,x_goal=0.0,y_goal=0.0,th_goal_abs=0.0,goaltype = "None"): """ Send a goal message to topic 'goal' """ """ Draw court """ global Goal_x,Goal_y graphics.canvas.delete(ALL) graphics.draw_court() if(goaltype == "newball"): th_goal = Ballbot_theta - math.radians(th_goal) # angle to goal in global coord # = angle of car in global coord - angle to goal from car (which is in [-90deg,90deg] x2 = Ballbot_x*100.0 + d_goal*math.cos(th_goal) y2 = Ballbot_y*100.0 + d_goal*math.sin(th_goal) th2 = 0 # doesn't matter for goal test # publish goal goal_msg = Goal() goal_msg.goaltype = String("newball") goal_msg.pose = Pose(x2,y2,th2) graphics.draw_point(x2,y2,th2,color='red') graphics.canvas.update_idletasks() pub_goal.publish(goal_msg) print "sent goal" Goal_x = x2 Goal_y = y2 elif(goaltype == "gotopose"): x2 = x_goal y2 = y_goal th2 = math.radians(th_goal_abs) # publish goal goal_msg = Goal() goal_msg.goaltype = String("gotopose") goal_msg.pose = Pose(x2,y2,th2) graphics.draw_point(x2,y2,th2,color='red') graphics.canvas.update_idletasks() pub_goal.publish(goal_msg) print "sent goal" Goal_x = x2 Goal_y = y2 """
def received_path(data): """ draw the new path """ graphics.canvas.delete(ALL) graphics.draw_court() graphics.draw_point(Goal_x,Goal_y,0,color='red') for element in data.poses: pose = element.pose graphics.draw_point(pose.x*100.0,pose.y*100.0,pose.theta,color='blue') #print "drew",pose.x*100.0,pose.y*100.0,pose.theta graphics.draw_point_directed(data.poses[0].pose.x*100.0,data.poses[0].pose.y*100.0,data.poses[0].pose.theta) graphics.draw_point_directed(data.poses[-1].pose.x*100.0,data.poses[-1].pose.y*100.0,data.poses[-1].pose.theta) graphics.canvas.update_idletasks() print "drew path"
def received_path(data): """ draw the new path """ graphics.canvas.delete(ALL) graphics.draw_court() graphics.draw_point(Goal_x, Goal_y, 0, color='red') for element in data.poses: pose = element.pose graphics.draw_point(pose.x * 100.0, pose.y * 100.0, pose.theta, color='blue') #print "drew",pose.x*100.0,pose.y*100.0,pose.theta graphics.draw_point_directed(data.poses[0].pose.x * 100.0, data.poses[0].pose.y * 100.0, data.poses[0].pose.theta) graphics.draw_point_directed(data.poses[-1].pose.x * 100.0, data.poses[-1].pose.y * 100.0, data.poses[-1].pose.theta) graphics.canvas.update_idletasks() print "drew path"
def startPlanner(x1,y1,th1,d_goal,th_goal): """ Start A* search to connect start to goal :) """ """ Draw court """ graphics.canvas.delete(ALL) util.costmap.draw_costmap() graphics.draw_court() th1 = math.radians(th1) """ state = util.LatticeNode(stateparams = (787.5,1715.0,5*math.pi/4,util.ROBOT_SPEED_MAX)) nextstate = util.LatticeNode(stateparams = (682.5,1820.0,math.pi,util.ROBOT_SPEED_MAX)) print "cost of action",util.cost(state,"RS_f",nextstate) print "cost of cell at 682.5,1820.0",util.costmap.cost_cell(682.5,1820.0) return """ (x1,y1,th1,v) = util.point_to_lattice(x1,y1,th1,util.ROBOT_SPEED_MAX) th_goal = th1 - math.radians(th_goal) # angle to goal in global coord # = angle of car in global coord - angle to goal from car (which is in [-90deg,90deg] x2 = x1 + d_goal*math.cos(th_goal) y2 = y1 + d_goal*math.sin(th_goal) th2 = 0 # doesn't matter for goal test v2 = util.ROBOT_SPEED_MAX (x2,y2,th2,v2) = util.point_to_lattice(x2,y2,th2,util.ROBOT_SPEED_MAX) if(x1 < 0) or (x1 > util.COURT_WIDTH) or (y1 < 0) or (y1 > util.COURT_LENGTH): print "Invalid start!" return if(x2 < 0) or (x2 > util.COURT_WIDTH) or (y2 < 0) or (y2 > util.COURT_LENGTH): print "Invalid goal!" return startNode = util.LatticeNode(stateparams = (x1,y1,th1,v)) goalNode = util.LatticeNode(stateparams = (x2,y2,th2,v2)) print "start ",startNode.get_stateparams()," goal ",goalNode.get_stateparams() (x1,y1,th1,v1) = startNode.get_stateparams() (x2,y2,th2,v2) = goalNode.get_stateparams() graphics.draw_point(x1,y1,th1,'red') graphics.draw_point(x2,y2,th2,'red') graphics.canvas.update_idletasks() #print "right turn ",util.turn_Right(startNode.get_stateparams(),'f') """ Print allowed car motions from startNode graphics.draw_segment(startNode,"F") graphics.draw_segment(startNode,"F3") graphics.draw_segment(startNode,"B") graphics.draw_segment(startNode,"R_f") graphics.draw_segment(startNode,"R_b") graphics.draw_segment(startNode,"L_f") graphics.draw_segment(startNode,"L_b") graphics.draw_segment(startNode,"SR_f") graphics.draw_segment(startNode,"SL_f") startNode2 = util.LatticeNode(stateparams = (490,490,math.pi/4,v)) graphics.draw_segment(startNode2,"RS_f") graphics.draw_segment(startNode2,"LS_f") graphics.draw_segment(startNode2,"F_diag") graphics.draw_segment(startNode2,"F_diag3") graphics.draw_segment(startNode2,"B_diag") return """ time_exec = time.time() goalNode = util.Astarsearch(startNode,goalNode) print "found goal! in",time.time() - time_exec,"s" path = [] if(goalNode != None): node = goalNode while(node.getParent()!= None): parent = node.getParent() print parent.get_stateparams(),node.getAction(),node.get_stateparams() path.append(node.getAction()) graphics.draw_segment(parent,node.getAction()) node = parent print "" path.reverse() graphics.draw_point(x1,y1,th1,'red') graphics.draw_point(x2,y2,th2,'red') #simulator.init_simulator(startNode,path) else: print "No path to goal!"
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()