def batch_node(f=None, dx=0, dz=0): global pub_command rospy.init_node("batch_publisher") location.init() rospy.on_shutdown(cleanUp) # wait for all channels to connect print "Connecting..." while pub_command.get_num_connections() == 0: pass pub_dx.publish(dx) pub_dz.publish(dz) if f == None: # if no command, get command from user's input print "LIVE MODE: enter commands or q to quit" command = None # initialize the command while True: command = sys.stdin.readline().strip() # get the command if command == "q": # if q was the command, quit break if cancel: print "in stop mode, command canceled" continue execute(*parse_command(command)) else: print str(f) for command in f.readlines(): if cancel: print "in stop mode, commands canceled" break execute(*parse_command(command)) cleanUp() # done and exit
def catcher(turn_first=False, no_wait=False): global pub_stop, pub_command, kinect_angle, depth_available rospy.init_node("balloon_catcher") rospy.Subscriber("/cur_tilt_angle", Float64, angleCallback) rospy.Subscriber("/camera/depth/image", Image, depthCallback) location.init() rospy.on_shutdown(cleanUp) while not depth_available: rospy.sleep(0.1) # get the balloon in the center of the screen(ball_tracker) if turn_first: targetBlob = scan(pub_command) else: targetBlob = None while targetBlob is None: centerOffset, targetBlob = get_blob_offset() KINECT_PIXELS_PER_DEGREE = 10 dist = None horizontal = 0 vertical = 0.001 v_prev = 0.001 V_THRESHOLD = 0.03 # meters last = default_timer() pub_sound.publish(ready) while v_prev - vertical <= V_THRESHOLD: print (v_prev, vertical, v_prev - vertical) centerOffset, targetBlob = get_blob_offset() now = default_timer() print("loop time {}".format(now - last)) last = now if centerOffset is None or targetBlob is None: continue # calculate angle from ground to camera-balloon line angle = kinect_angle + ((depth_map.height/2) - targetBlob.y)/KINECT_PIXELS_PER_DEGREE # get the distance to the balloon dist = getDepth(targetBlob.x, targetBlob.y) v_prev = vertical if isnan(dist): vertical = 0 horizontal = 1.1 else: vertical = getOpposite(angle, dist) horizontal = getAdjacent(angle, dist) if no_wait: break pub_sound.publish(ready) print (v_prev, vertical, v_prev - vertical) print "balloon falling {} meters from camera".format(dist) print "height {} meters, estimated landing point {} meters".format(vertical, horizontal) # move the calculated distance # we could use batch command's execute but the trick will be the ball won't be in the center all the time when it falls # so we might need to use the tracking blob with a higher sensitivity. location.resetOdom() command = zero() SLEEP = 0.01 DELTA_X = 0.9 * SLEEP Z_MAX = 0.6 IMAGE_WIDTH = 640 X_TURN_MAX = 0.7 while(location.currentLocation[0] < horizontal): if command.linear.x < 1: command.linear.x += DELTA_X if command.linear.x > 1: command.linear.x = 1 command.angular.z = 0 centerOffset, _ = get_blob_offset() if centerOffset is None: pub_command.publish(command) continue speed = 50 * centerOffset/float(IMAGE_WIDTH) # calculate the right amount of speed for the command if centerOffset > 20: # if the offset is bigger than 20 # print "{} LEFT".format(abs(centerOffset)) command.angular.z = min(Z_MAX, speed) # turn left and follow # print([command.angular.z, centerOffset/rawBlobs.image_width]) elif centerOffset < -20: # if the offset is smaller than -20 # print "{} RIGHT".format(abs(centerOffset)) command.angular.z = max(-Z_MAX, speed) # turn right and follow the ball # print([command.angular.z, centerOffset/rawBlobs.image_width]) if abs(command.angular.z) > 0.2 and command.linear.x > X_TURN_MAX: command.linear.x = X_TURN_MAX # print "turning" pub_command.publish(command) # print "x {}, z {}".format(command.linear.x, command.angular.z) rospy.sleep(SLEEP) command = zero() pub_command.publish(command)
def soccer(): global pub_command, pub_stop, TARGET_OFFSET, file rospy.init_node("soccer_node") rospy.Subscriber("/emergency_stop", Empty, closer) location.init() rospy.on_shutdown(cleanUp) file = open("soccer.log", "w") print "Connecting..." while pub_command.get_num_connections() == 0: pass x = 0 y = 0 theta = 0 true_angle = 0 file.write("{} {}\n".format(x, y)) print["start", x, y] # scan from current position for ball and goal ball_angle_1, goal_angle_1 = scan(pub_command) rospy.sleep(1) # turn to face 45 degrees past ball away from goal if ball_angle_1 < 0: ball_angle_1 += 360 if goal_angle_1 < 0: goal_angle_1 += 360 direction = 1 delta = goal_angle_1 - ball_angle_1 if delta < -180 or (delta > 0 and delta < 180): direction = -1 target_angle = ball_angle_1 + direction * 45 print "--- SCAN 1 RESULTS ---" print "x: {} y: {}, goal @ {}, ball @ {}".format(x, y, goal_angle_1, ball_angle_1) goal_vector_1 = Line(x=x, y=y, theta=goal_angle_1, useDegrees=True) ball_vector_1 = Line(x=x, y=y, theta=ball_angle_1, useDegrees=True) _, _, theta = location.currentLocation if theta < 0: theta += 360 move_angle = target_angle - theta execute(0, move_angle, 0.6) rospy.sleep(1) # move to new location move_dist = 0.3 execute(move_dist, 0, 0.5) print "--- REALIGNING FOR SCAN 2 ---" print "odom @ {}, angle from scan 1 start @ {}".format( location.currentLocation[2], target_angle) true_angle = target_angle scan_2_vector = Line(x=0, y=0, theta=true_angle, useDegrees=True) x, y = scan_2_vector.findPointFrom(0, 0, move_dist) file.write("{} {}\n".format(x, y)) print["scan_2", x, y] # get new position _, _, theta = location.currentLocation theta += true_angle # scan from current position for ball and goal ball_angle_2, goal_angle_2 = scan(pub_command) ball_angle_2 += true_angle goal_angle_2 += true_angle goal_vector_2 = Line(x=x, y=y, theta=goal_angle_2, useDegrees=True) ball_vector_2 = Line(x=x, y=y, theta=ball_angle_2, useDegrees=True) # calculate intersections goal_x, goal_y = goal_vector_1.intersect(goal_vector_2) ball_x, ball_y = ball_vector_1.intersect(ball_vector_2) print "--- SCAN 2 RESULTS ---" print "x: {}, y: {}, goal @ {}, ball @ {}".format(x, y, goal_angle_2, ball_angle_2) print "--- GOAL LOCATION ---" print "x: {}, y: {}".format(goal_x, goal_y) print "--- BALL LOCATION ---" print "x: {}, y: {}".format(ball_x, ball_y) file.write("{} {}\n".format(ball_x, ball_y)) file.write("{} {}\n".format(goal_x, goal_y)) print["ball", ball_x, ball_y] print["goal", goal_x, goal_y] # find line from ball to goal approach_vector = Line(x1=goal_x, y1=goal_y, x2=ball_x, y2=ball_y) TARGET_OFFSET = 0.5 # select point on approach vector for robot to start target_x, target_y = approach_vector.findPointFrom(ball_x, ball_y, TARGET_OFFSET) alt_x = ball_x * 2 - target_x alt_y = ball_y * 2 - target_y file.write("{} {}\n".format(target_x, target_y)) print["kick", target_x, target_y] file.write("{} {}\n".format(alt_x, alt_y)) print["kick_alt", alt_x, alt_y] d_ball_target = distance(ball_x, ball_y, target_x, target_y) d_ball_goal = distance(ball_x, ball_y, goal_x, goal_y) d_goal_target = distance(goal_x, goal_y, target_x, target_y) d_goal_alt = distance(goal_x, goal_y, alt_x, alt_y) if d_goal_alt > d_goal_target: print "--- ALT SELECTED ---" target_x = alt_x target_y = alt_y else: print "--- FIRST TARGET SELECTED ---" print "x: {}, y: {}".format(target_x, target_y) # find way to move from current position to target pt dist = distance(x, y, target_x, target_y) movement_vector = Line(x=x, y=y, x2=target_x, y2=target_y) angle = movement_vector.angle(useDegrees=True) if angle < 0: angle += 360 # check if the angle is correct or opposite of correct if ball_angle_2 > goal_angle_2: print "--- USING ALTERNATE ANGLE ---" if angle < ball_angle_2: angle += 180 if angle > ball_angle_2 + 180: angle -= 180 print "--- APPROACHING SHOT LOCATION ---" print "angle to shot location @ {}".format(angle) _, _, theta = location.currentLocation theta += true_angle execute(0, angle - theta, 0.6, reset=True) execute(dist, 0, 0.6, reset=True) print "--- REALIGNING FOR SHOT ---" print "odom @ {}, angle from scan 2 start @ {}, angle from scan 1 start @ {}".format( location.currentLocation[2], angle, angle + true_angle) # scan for ball(and goal) ball_angle_fin, goal_angle_fin = scan(pub_command) # create background thread to stop robot once ball is hit from threading import Thread stop_thread = Thread(target=stopper) stop_thread.start() # make the shot execute(TARGET_OFFSET + 2, 0, 1, reset=True) stop_thread.join() file.close() file = None
import location import store_db import places import ir #import camera import time store_db.init("nil", "nil", "Start") flag = 0; while True: if flag == 0: location.init() flag = 1; else location.track() if ir.track() == 1: loc_coords = location.find() loc_name = places.find(loc_coords) store_db.init(loc_coords, loc_name, "Lane crossed") time.sleep(1)
def init(): rospy.init_node("test") # initialize the node location.init() # init the location rospy.on_shutdown(cleanUp) rospy.Subscriber("/blobs", Blobs, setRawBlobs) # subscribe to blobs