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