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()
