def main(nh):
boat = yield boat_scripting.get_boat(nh)
while True:
yield util.sleep(.1)
time_left_str = yield util.nonblocking_raw_input('Enter time left: (e.g. 5:40) ')
try:
m, s = time_left_str.split(':')
time_left = 60 * int(m) + int(s)
except Exception:
traceback.print_exc()
else:
end_time = time.time() + time_left
del time_left
break
print
print 'WAIT 10 SECONDS'
print
while True:
yield util.sleep(.1)
course = yield util.nonblocking_raw_input('Course: (A or B) ')
if course in ['A', 'B', 'pool']:
break
try:
yield util.wrap_timeout(main_list(nh, boat, course), max(0, end_time - time.time()))
except Exception:
traceback.print_exc()
yield fail_list(nh, boat)
def main(nh):
sub = yield sub_scripting.get_sub(nh)
sub.move.go(linear=[0.25, 0, 0])
yield sub.visual_approach('forward', 'shooter', size_estimate=7*.0254, desired_distance=1.5, selector=select_by_body_direction([0,1,0]))
yield util.sleep(5)
yield sub.move.forward(.5).go()
yield sub.move.up(5*.0254).go()
yield sub.move.right(3.5*.0254).go()
yield util.sleep(5)
yield sub.move.backward(2).go()
yield sub.fire_left_torpedo()
yield sub.visual_approach('forward', 'shooter', size_estimate=7*.0254, desired_distance=1.5, selector=select_by_body_direction([0,-1,0]))
yield util.sleep(5)
yield sub.move.forward(.5).go()
yield sub.move.up(5*.0254).go()
yield sub.move.left(3.5*.0254).go()
yield util.sleep(5)
yield sub.move.backward(2).go()
yield sub.fire_right_torpedo()
"""yield sub.move.right(.6).go()
def get_distance_from_object(self, radius):
temp_distance = 0
avg_distance = 0
shortest_distance = 100
farthest_distance = 0
return_array = []
hold = []
while len(hold) <= 0:
# get pointcloud
pointcloud = yield self.get_pointcloud()
yield util.sleep(.2) # sleep to avoid tooPast errors
pointcloud_base = yield self.to_baselink(pointcloud)
yield util.sleep(.2) # sleep to avoid tooPast errors
# Filter lidar data to only data right in front of the boat
hold = filter(lambda x: abs(x[1]) < radius, pointcloud_base)
# Calculate several distances between target and boat
for x in range(len(hold)):
dist = hold[x]
temp_distance += dist[0]
# Check and assign the closest object to the boat
if dist[0] < shortest_distance: shortest_distance = dist[0]
if dist[0] > farthest_distance: farthest_distance = dist[0]
avg_distance = temp_distance / len(hold)
shortest_distance = shortest_distance
farthest_distance = farthest_distance
return_array.append(avg_distance)
return_array.append(shortest_distance)
return_array.append(farthest_distance)
defer.returnValue(return_array)
def main(nh):
boat = yield boat_scripting.get_boat(nh)
while True:
yield util.sleep(.1)
time_left_str = yield util.nonblocking_raw_input(
'Enter time left: (e.g. 5:40) ')
try:
m, s = time_left_str.split(':')
time_left = 60 * int(m) + int(s)
except Exception:
traceback.print_exc()
else:
end_time = time.time() + time_left
del time_left
break
print
print 'WAIT 10 SECONDS'
print
while True:
yield util.sleep(.1)
course = yield util.nonblocking_raw_input('Course: (A or B) ')
if course in ['A', 'B', 'pool']:
break
try:
yield util.wrap_timeout(main_list(nh, boat, course),
max(0, end_time - time.time()))
except Exception:
traceback.print_exc()
yield fail_list(nh, boat)
def main(nh):
print "starting shooter"
sub = yield sub_scripting.get_sub(nh)
fwd_move = sub.move.go(linear=[0.25, 0, 0])
try:
obj = yield util.wrap_timeout(sub.visual_approach('forward', 'shooter/hole', size_estimate=5*.0254, desired_distance=1.5, selector=select_by_body_direction([0,1,0])), 30)
finally:
yield fwd_move.cancel()
start = sub.move
#try:
# yield util.wrap_timeout(align(sub), 30)
#except Exception:
# yield start.go() # go back because alignment failed
yield sub.visual_approach('forward', 'shooter/hole', size_estimate=5*.0254, desired_distance=1.0, selector=select_by_body_direction([0,1,0]))
yield util.sleep(5)
yield sub.move.forward(.3).go()
yield sub.move.up(5*.0254).go()
yield sub.move.right(3.5*.0254).go()
yield sub.fire_left_torpedo()
yield sub.move.backward(2.0).go()
print 'going to second hole'
yield sub.visual_approach('forward', 'shooter/hole', size_estimate=5*.0254, desired_distance=1.0, selector=select_by_body_direction([0,-1,0]))
yield util.sleep(5)
yield sub.move.forward(0.3).go()
yield sub.move.up(5*.0254).go()
yield sub.move.left(3.5*.0254).go()
yield sub.fire_right_torpedo()
yield sub.move.backward(1.0).go()
"""yield sub.move.right(.6).go()
def get_distance_from_object(self, radius):
temp_distance = 0
avg_distance = 0
shortest_distance = 100
farthest_distance = 0
return_array = []
hold = []
while len(hold) <= 0:
# get pointcloud
pointcloud = yield self.get_pointcloud()
yield util.sleep(.2) # sleep to avoid tooPast errors
pointcloud_base = yield self.to_baselink(pointcloud)
yield util.sleep(.2) # sleep to avoid tooPast errors
# Filter lidar data to only data right in front of the boat
hold = filter(lambda x: abs(x[1]) < radius, pointcloud_base)
# Calculate several distances between target and boat
for x in range(len(hold)):
dist = hold[x]
temp_distance += dist[0]
# Check and assign the closest object to the boat
if dist[0] < shortest_distance: shortest_distance = dist[0]
if dist[0] > farthest_distance: farthest_distance = dist[0]
avg_distance = temp_distance/len(hold)
shortest_distance = shortest_distance
farthest_distance = farthest_distance
return_array.append(avg_distance)
return_array.append(shortest_distance)
return_array.append(farthest_distance)
defer.returnValue(return_array)
def main(nh):
sub = yield sub_scripting.get_sub(nh)
#yield sub.lower_down_grabber()
while True:
yield util.sleep(2)
yield sub.lower_down_grabber()
yield sub.open_down_grabber()
yield util.sleep(2)
def main():
nh = yield txros.NodeHandle.from_argv('thruster_calibrator')
pub = nh.advertise('/thrusters/command/test', ThrusterCommand)
port = yield nh.get_param('~port')
lc = LoadCell(nh, port)
power_holder = [0]
@util.cancellableInlineCallbacks
def set_thruster_thread():
while True:
pub.publish(ThrusterCommand(force=power_holder[0]))
yield util.sleep(.1)
set_thruster_thread()
for i in xrange(1, -20, -1):
power = i / 20
power_holder[0] = power
yield util.sleep(1)
res = []
for i in xrange(-20, 20 + 1):
power = i / 20
power_holder[0] = power
yield util.sleep(2)
s = 0
for j in xrange(400):
s += (yield lc.measurement_event.get_deferred())[0]
s /= 400
if i == 0: zero_s = s
print power, s
res.append((s, power))
print[(-(x - zero_s), y) for x, y in res]
power_holder[0] = 0
yield util.sleep(1)
def interoperability(nh, boat, s):
global docking_info
global images_info
global sent_image
print "Moving to position to begin interoperability"
s.set_current_challenge('interop')
# Get the images
images_info = yield images_info
print 'JSON says: \n\n' + str(images_info) + '\n\n'
images_path = images_info.file_path
images_count = images_info.image_count
try:
yield util.wrap_timeout(go_to_ecef_pos.main(nh, QUAD[course]), ECEF_TIME)
except:
print 'ECEF timeout'
to_get_send = ['a','a']
to_get_send = ssocr_simple.main(images_path)
# Send the image but don't yield that way we can move while it sends
print "Sending image", to_get_send
sent_image = yield s.send_image_info(to_get_send[1], to_get_send[0]) # FOUR the sake of testing
print "Removing temporary work path"
shutil.rmtree(images_path)
# Wait here for show
print 'Chilling at interoperability challange'
yield util.sleep(5)
def fail_list(nh, boat):
try:
print 'fail start'
yield util.sleep(1)
print 'fail end'
finally:
print 'fail finally'
def main(nh, target = None, orient_target = None):
sub = yield sub_scripting.get_sub(nh)
picked_up_target = False
align_target = target
if align_target == None: align_target = 'delorean'
orientation_align_target = orient_target
if orientation_align_target == None: orientation_align_target = 'delorean'
x_location = 0
y_location = 0
while not rospy.is_shutdown():
yield sub.orient_and_align(align_target, orientation_align_target, MOVE_SCALE, ANGULAR_TOLERANCE, ORIENTATION_TOLERANCE)
# Move down until distance from obbject is 1 meter
yield move_down(sub)
while picked_up_target == False:
yield sub.close_gripper()
yield sub.move.up(1).go()
# Scan ten times to verify that we have picked up object
for x in xrange(1,10):
target_scan = yield sub.get_target_location(align_target)
yield util.sleep(.1)
if target_scan.x == 0 and target_scan.y == 0: picked_up_target = True
def fail_list(nh, boat):
try:
print 'fail start'
yield util.sleep(1)
print 'fail end'
finally:
print 'fail finally'
def interoperability(nh, boat, s):
global docking_info
global images_info
global sent_image
print "Moving to position to begin interoperability"
s.set_current_challenge('interop')
# Get the images
images_info = yield images_info
print 'JSON says: \n\n' + str(images_info) + '\n\n'
images_path = images_info.file_path
images_count = images_info.image_count
try:
yield util.wrap_timeout(go_to_ecef_pos.main(nh, QUAD[course]),
ECEF_TIME)
except:
print 'ECEF timeout'
to_get_send = ['a', 'a']
to_get_send = ssocr_simple.main(images_path)
# Send the image but don't yield that way we can move while it sends
print "Sending image", to_get_send
sent_image = yield s.send_image_info(
to_get_send[1], to_get_send[0]) # FOUR the sake of testing
print "Removing temporary work path"
shutil.rmtree(images_path)
# Wait here for show
print 'Chilling at interoperability challange'
yield util.sleep(5)
def get_weight(sub):
yield util.sleep(10)
start = time.time()
res = []
while time.time() < start + 5:
res.append((yield sub.get_z_force()))
defer.returnValue(sum(res) / len(res))
def main(nh):
s = yield json_server_proxy.get_server(nh)
try:
#url_was_set = s.interact('http://ec2-52-7-253-202.compute-1.amazonaws.com:80','openTest')
url = raw_input('url: ')
course = raw_input('Course:')
url_was_set = yield s.interact('http://' + url, course)
if url_was_set.was_set:
print "Url and course were set succesfully"
challenge_was_set = (yield s.set_current_challenge('gates')).was_set
if challenge_was_set:
print "Challenge was set succesfully"
yield s.end_run()
print 'Ended last run'
run_started = (yield s.start_run()).success
if run_started:
print "Run started succesfully"
images = yield s.get_server_images()
print images
sent_image = yield s.send_image_info('0.png', 'ZERO')
print sent_image
dock = yield s.get_dock_info()
print dock
gates = yield s.get_gate_info()
print gates
color = 'blue'
for c in ['blue', 'green', 'red', 'yellow', 'black']:
check = yield s.send_buoy_info(c)
if check.is_right_buoy:
print 'Got the right buoy: ' + c
color = c
break
else:
print 'Got the wrong buoy: ' + c
print test
print '____Beat?____'
yield util.sleep(60)
finally:
yield s.end_run()
print 'Ended run'
def _work():
perms = set(itertools.product(colors, repeat=3))
while perms:
perm = random.choice(list(perms))
try:
res = yield ci.report_light_sequence(course, perm)
if res: break
if not res: perms.remove(perm)
except: traceback.print_exc()
yield util.sleep(1)
def main(nh):
sub = yield sub_scripting.get_sub(nh)
print "firing left torpedo"
yield sub.fire_left_torpedo()
print "fired, waiting 3 seconds"
yield util.sleep(3)
print "firing right torpedo"
yield sub.fire_right_torpedo()
print "fired right torpedo"
def main(nh):
sub = yield sub_scripting.get_sub(nh)
print "lowering impaler"
yield sub.lower_impaler()
print "lowered, waiting 3 seconds"
yield util.sleep(3)
print "raising impaler"
yield sub.raise_impaler()
print "raised impaler"
def float(self):
while True:
self._send_constant_wrench_service(SendConstantWrenchRequest(
wrench=Wrench(
force=Vector3(0, 0, 0),
torque=Vector3(0, 0, 0),
),
lifetime=genpy.Duration(1),
))
yield util.sleep(.5)
def go():
print 'a'
last_best = None
while True:
scan = yield boat.get_scan()
pose = boat.odom
best = None
best2 = None
angles = numpy.linspace(scan.angle_min, scan.angle_max, len(scan.ranges))
pos = lambda i: scan.ranges[i]*numpy.array([math.cos(angles[i]), math.sin(angles[i]), 0])
for i in xrange(len(scan.ranges)//6, len(scan.ranges)*5//6):
if not (scan.range_min <= scan.ranges[i] <= scan.range_max):
continue
for j in xrange(i, len(scan.ranges)*5//6):
if not (scan.range_min <= scan.ranges[j] <= scan.range_max):
continue
center = (pos(i) + pos(j))/2
vec = pos(j) - pos(i)
if numpy.linalg.norm(vec) < 1 or numpy.linalg.norm(vec) > 5:
continue
bad = False
for k in xrange(int(i*.75+j*.25), int(i*.25+j*.75)):
if not (scan.range_min <= scan.ranges[k] <= scan.range_max):
continue
if numpy.linalg.norm(pos(k) - center) < 5:
bad = True
break
if bad: continue
score = center[1] if direction == 'left' else -center[1]
if best is None or score > best_score:
best = center
best_score = score
best2 = i, j, center, vec
print best, best2
print scan.ranges[best2[0]:best2[1]+3]
if best is not None:
last_best = pose.relative(best).position
if last_best is None:
df = boat.move.forward(5).go()
else:
df = boat.move.set_position(last_best).go()
if numpy.linalg.norm(last_best - boat.pose.position) < 5:
print 'waiting'
yield df
break
yield util.sleep(.1)
def _work():
perms = set(itertools.product(colors, repeat=3))
while perms:
perm = random.choice(list(perms))
try:
res = yield ci.report_light_sequence(course, perm)
if res: break
if not res: perms.remove(perm)
except:
traceback.print_exc()
yield util.sleep(1)
def float(self):
while True:
self._send_constant_wrench_service(
SendConstantWrenchRequest(
wrench=Wrench(
force=Vector3(0, 0, 0),
torque=Vector3(0, 0, 0),
),
lifetime=genpy.Duration(1),
))
yield util.sleep(.5)
def main(nh, boat=None):
print "Q"
if boat is None:
boat = yield boat_scripting.get_boat(nh)
while True:
print 'find gates'
angle = yield boat.get_start_gate_vision()
print "raw angle", angle
distance = 0
#yield boat.move.as_MoveToGoal([3,0,0],angle).go()
# ADDED BBY ZACH -- Measures the distance the boat has gone forward to tell when done
# Breaks and returns true so that we know we actually got done.
# Not a great implimentation but works for now
# Change depending on how far apart the bouys are
if distance > 30:
print "Exiting Gates"
break
if abs(angle) < (numpy.pi/12):
print "forward"
yield boat.move.forward(6).go()
yield util.sleep(0.5)
print "forward command sent"
distance += 6
elif angle < 0:
print "turn_left: ", angle/5
yield boat.move.turn_left(abs(angle/5)).go()
yield util.sleep(0.5)
elif angle > 0:
print "turn_right: ", angle/5
yield boat.move.turn_right(angle/5).go()
yield util.sleep(0.5)
'''
def _send_result(ci, course, lat, lon):
colors = set(['yellow', 'blue', 'black', 'green', 'red'])
while colors:
color = random.choice(list(colors))
try:
res = yield ci.send_pinger_answer(course, color, lat, lon)
print res
if res: return
if not res: colors.remove(color)
except:
traceback.print_exc()
yield util.sleep(5)
def main(nh, boat=None):
print "Q"
if boat is None:
boat = yield boat_scripting.get_boat(nh)
while True:
print 'find gates'
angle = yield boat.get_start_gate_vision()
print "raw angle", angle
distance = 0
#yield boat.move.as_MoveToGoal([3,0,0],angle).go()
# ADDED BBY ZACH -- Measures the distance the boat has gone forward to tell when done
# Breaks and returns true so that we know we actually got done.
# Not a great implimentation but works for now
# Change depending on how far apart the bouys are
if distance > 30:
print "Exiting Gates"
break
if abs(angle) < (numpy.pi / 12):
print "forward"
yield boat.move.forward(6).go()
yield util.sleep(0.5)
print "forward command sent"
distance += 6
elif angle < 0:
print "turn_left: ", angle / 5
yield boat.move.turn_left(abs(angle / 5)).go()
yield util.sleep(0.5)
elif angle > 0:
print "turn_right: ", angle / 5
yield boat.move.turn_right(angle / 5).go()
yield util.sleep(0.5)
'''
def deploy_hydrophone(self):
#INITIALLY WHEN THE BOAT STARTS THE CABLE SHOULD BE FEED OVER THE TOP OF THE SERVO
deploy_msg=DynamixelFullConfig()
deploy_msg.id=4 #id 4 is the stern servo for the hydrophones
deploy_msg.led=0
deploy_msg.goal_position=-2*math.pi
deploy_msg.moving_speed=1.4 # 1.4 rad/s~22 rpm
deploy_msg.torque_limit=173 # 173/1023 is about 17% torque
deploy_msg.goal_acceleration=20
deploy_msg.control_mode=DynamixelFullConfig.CONTINUOUS_ANGLE
deploy_msg.goal_velocity=1.4
for i in xrange(100):
self.servo_full_config_pub.publish(deploy_msg)
yield util.sleep(8.5/100)
def deploy_hydrophone(self):
#INITIALLY WHEN THE BOAT STARTS THE CABLE SHOULD BE FEED OVER THE TOP OF THE SERVO
deploy_msg = DynamixelFullConfig()
deploy_msg.id = 4 #id 4 is the stern servo for the hydrophones
deploy_msg.led = 0
deploy_msg.goal_position = -2 * math.pi
deploy_msg.moving_speed = 1.4 # 1.4 rad/s~22 rpm
deploy_msg.torque_limit = 173 # 173/1023 is about 17% torque
deploy_msg.goal_acceleration = 20
deploy_msg.control_mode = DynamixelFullConfig.CONTINUOUS_ANGLE
deploy_msg.goal_velocity = 1.4
for i in xrange(100):
self.servo_full_config_pub.publish(deploy_msg)
yield util.sleep(8.5 / 100)
def retract_hydrophone(self):
#WHEN THE HYDROPHONES RETRACT CABLE SHOULD FEED BACK OVER THE TOP OF THE SERVO
deploy_msg = DynamixelFullConfig()
deploy_msg.id = 4 #id 4 is the stern servo for the hydrophones
deploy_msg.led = 0
deploy_msg.goal_position = 4.3 # 2.4 rad/s~22 rpm NOTE: we explicitly retract to pi to try and avoid being at the 0/2*PI boundary on a powerup
deploy_msg.moving_speed = 1.4 # 1.4 rad/s~22 rpm
deploy_msg.torque_limit = 143 # 143/1023 is about 14% torque (so we don't break the rope if someone didn't feed them correctly to start)
deploy_msg.goal_acceleration = 20
deploy_msg.control_mode = DynamixelFullConfig.CONTINUOUS_ANGLE
deploy_msg.goal_velocity = 1.4
self.servo_full_config_pub.publish(deploy_msg)
for i in xrange(100):
self.servo_full_config_pub.publish(deploy_msg)
yield util.sleep(20 / 100)
def retract_hydrophone(self):
#WHEN THE HYDROPHONES RETRACT CABLE SHOULD FEED BACK OVER THE TOP OF THE SERVO
deploy_msg=DynamixelFullConfig()
deploy_msg.id=4 #id 4 is the stern servo for the hydrophones
deploy_msg.led=0
deploy_msg.goal_position=4.3 # 2.4 rad/s~22 rpm NOTE: we explicitly retract to pi to try and avoid being at the 0/2*PI boundary on a powerup
deploy_msg.moving_speed=1.4 # 1.4 rad/s~22 rpm
deploy_msg.torque_limit=143 # 143/1023 is about 14% torque (so we don't break the rope if someone didn't feed them correctly to start)
deploy_msg.goal_acceleration=20
deploy_msg.control_mode=DynamixelFullConfig.CONTINUOUS_ANGLE
deploy_msg.goal_velocity=1.4
self.servo_full_config_pub.publish(deploy_msg)
for i in xrange(100):
self.servo_full_config_pub.publish(deploy_msg)
yield util.sleep(20/100)
def hydrophone_align(self, frequency):
good = 0
while True:
x = self.float()
try:
yield util.sleep(.5)
ping_return = yield self.get_processed_ping(frequency)
finally:
x.cancel()
print ping_return
if ping_return.declination > 1.2:
good += 1
if good > 4: return
elif abs(ping_return.heading) > math.radians(30):
good = 0
yield self.move.yaw_left(ping_return.heading).go()
else:
good = 0
yield self.move.forward(2).go()
def hydrophone_align(self, frequency):
good = 0
while True:
x = self.float()
try:
yield util.sleep(.5)
ping_return = yield self.get_processed_ping(frequency)
finally:
x.cancel()
print ping_return
if ping_return.declination > 1.2:
good += 1
if good > 4: return
elif abs(ping_return.heading) > math.radians(30):
good = 0
yield self.move.yaw_left(ping_return.heading).go()
else:
good = 0
yield self.move.forward(2).go()
def main(nh, location='lake_alice'):
boat = yield boat_scripting.get_boat(nh)
boat.set_current_challenge('test')
try:
while True:
print 'Float 1 sec'
boat.float_on()
yield util.sleep(1)
print 'Spin'
boat.float_off()
print 'O pose: ', boat.odom.position
print 'O orien: ', boat.odom.orientation
print 'P pose: ', boat.pose.position
print 'P orien: ', boat.pose.orientation
#yield boat.move.set_position(boat.odom.position).set_orientation(boat.odom.orientation).go()
#yield boat.move.turn_left_deg(90).go()
finally:
boat.default_state()
def main(nh, min_freq, max_freq):
boat = yield boat_scripting.get_boat(nh)
while True:
ping_return = None
while ping_return is None:
try:
print 'Float'
boat.float_on()
yield util.sleep(.5)
print 'Listen'
ping_return = yield util.wrap_timeout(
boat.get_processed_ping((min_freq, max_freq)), 20)
except Exception:
# Timeout rotate 30 deg
print 'Timeout no ping rotating 30 degrees left'
boat.float_off()
#yield boat.move.set_position(boat.odom.position).set_orientation(boat.odom.orientation)
yield boat.move.yaw_left_deg(30).go()
print 'Rotated 30 degrees'
boat.float_off()
# Changed message type to handle this. Conversion is carried out in the hydrophones script in
# software_common/hydrophones/scripts
print 'Ping is: ', ping_return
print 'I say heading is: ', ping_return.heading
print 'I say declination is: ', ping_return.declination
if ping_return.declination > 1.2:
good += 1
if good > 4:
print 'Success!'
return
elif abs(ping_return.heading) > math.radians(30):
good = 0
print 'Turn to ping'
yield boat.move.yaw_left(ping_return.heading).go()
else:
good = 0
print 'Move towards ping'
yield boat.move.forward(2).go()
def main(nh, min_freq, max_freq):
boat = yield boat_scripting.get_boat(nh)
while True:
ping_return = None
while ping_return is None:
try:
print 'Float'
boat.float_on()
yield util.sleep(.5)
print 'Listen'
ping_return = yield util.wrap_timeout(boat.get_processed_ping((min_freq, max_freq)), 20)
except Exception:
# Timeout rotate 30 deg
print 'Timeout no ping rotating 30 degrees left'
boat.float_off()
#yield boat.move.set_position(boat.odom.position).set_orientation(boat.odom.orientation)
yield boat.move.yaw_left_deg(30).go()
print 'Rotated 30 degrees'
boat.float_off()
# Changed message type to handle this. Conversion is carried out in the hydrophones script in
# software_common/hydrophones/scripts
print 'Ping is: ', ping_return
print 'I say heading is: ', ping_return.heading
print 'I say declination is: ', ping_return.declination
if ping_return.declination > 1.2:
good += 1
if good > 4:
print 'Success!'
return
elif abs(ping_return.heading) > math.radians(30):
good = 0
print 'Turn to ping'
yield boat.move.yaw_left(ping_return.heading).go()
else:
good = 0
print 'Move towards ping'
yield boat.move.forward(2).go()
def main(nh):
sub = yield sub_scripting.get_sub(nh)
orig_depth = -sub.pose.position[2]
#yield sub.move.forward(1).go()
print "aligning down"
dist = yield sub.get_dvl_range()
fwd_move = sub.move.go(linear=[0.25, 0, 0])
try:
yield sub.visual_align('down', 'bins/all', distance_estimate=dist-.3, turn=True, angle=math.radians(45))
finally:
fwd_move.cancel()
dist = yield sub.get_dvl_range()
yield sub.visual_align('down', 'bins/all', distance_estimate=dist-.3, turn=False)
#yield sub.move.down(dist-.3 - 2.5).go()
print "aligned down"
yield sub.move.down(dist - 3.2).go()
dist = yield sub.get_dvl_range()
centered = sub.move
for x in ["1", "4"]:
print 'going to', x
yield centered.go()
print "aligning single", x
dist = yield sub.get_dvl_range()
yield sub.visual_align('down', 'bins/single', distance_estimate=dist-.3, selector=select(x), turn=False)
print 'done'
yield sub.move.down(dist - 2).go()
yield sub.visual_align('down', 'bins/single', distance_estimate=2, selector=select_centered, turn=True)
yield sub.move.backward(.25).go()
print 'dropping'
yield sub.drop_ball()
yield util.sleep(3)
print 'done dropping'
yield sub.move.depth(orig_depth).go()
def main_list(nh, boat, course):
try:
print 'main start'
#yield boat.move.forward(3).go()
try:
dock_item = yield ci.start_automated_docking(course)
except Exception:
traceback.print_exc()
dock_item = 'triangle'
print 'Defaulting to dock_item:', dock_item
print 'dock_item:', dock_item
try:
gates = yield ci.start_obstacle_avoidance(course)
except Exception:
traceback.print_exc()
gates = random.choice(['1', '2',
'3']), random.choice(['X', 'Y', 'Z'])
print 'Defaulting to gates:', gates
print 'gates:', gates
print 'Activating light sequence'
try:
res = yield ci.activate_light_sequence(course)
except Exception:
traceback.print_exc()
print 'LIGHT SEQUENCE FAILED! WARNING!'
#yield util.sleep(5)
else:
print 'Result:', res
yield util.sleep(2.0)
colors = ["red", "green", "blue", "yellow"]
@util.cancellableInlineCallbacks
def _work():
perms = set(itertools.product(colors, repeat=3))
while perms:
perm = random.choice(list(perms))
try:
res = yield ci.report_light_sequence(course, perm)
if res: break
if not res: perms.remove(perm)
except:
traceback.print_exc()
yield util.sleep(1)
_work()
print 'Running gate2'
yield boat.move.forward(75).go()
#try:
# yield util.wrap_timeout(gate2.main(nh, 'left' if course == 'B' else 'right'), 60*2)
#except Exception:
# traceback.print_exc()
try:
yield util.wrap_timeout(
do_obstacle_course(nh, boat, course, gates), 2 * 60)
except Exception:
traceback.print_exc()
# safe point
print 'Going to safe point 1'
yield boat.go_to_ecef_pos(
dict(
pool=[1220416.51743, -4965356.4575, 3799838.03177],
A=ll(36.802040, -76.191835), # from google earth
B=ll(36.801972, -76.191849), # from google earth
)[course])
try:
yield util.wrap_timeout(do_dock(nh, boat, course, dock_item),
60 * 4)
except Exception:
traceback.print_exc()
# safe point
print 'Going to safe point 1'
yield boat.go_to_ecef_pos(
dict(
pool=[1220416.51743, -4965356.4575, 3799838.03177],
A=ll(36.802040, -76.191835), # from google earth
B=ll(36.801972, -76.191849), # from google earth
)[course])
# safe point
print 'Going to safe point 3'
yield boat.go_to_ecef_pos(
dict(
pool=[1220416.51743, -4965356.4575, 3799838.03177],
A=ll(36.802185, -76.191492), # from google earth
B=ll(36.801972, -76.191849), # from google earth
)[course])
# safe point
print 'Going to safe point 4'
yield boat.go_to_ecef_pos(
dict(
pool=[1220416.51743, -4965356.4575, 3799838.03177],
A=ll(36.802365, -76.191650), # from google earth
B=ll(36.801972, -76.191849), # from google earth
)[course])
print 'going to pinger'
yield boat.go_to_ecef_pos(
dict(
A=ll(36.802720, -76.191470),
B=ll(36.80175, -76.19230),
)[course])
freq = dict(
A=27e3,
B=32e3,
)[course]
print 'acoustic_beacon'
yield acoustic_beacon.main(nh, ci, course, freq)
yield boat.go_to_ecef_pos(
dict(
A=ll(36.802720, -76.191470),
B=ll(36.80175, -76.19230),
)[course])
print 'Going to safe point 4'
yield boat.go_to_ecef_pos(
dict(
pool=[1220416.51743, -4965356.4575, 3799838.03177],
A=ll(36.802365, -76.191650), # from google earth
B=ll(36.801972, -76.191849), # from google earth
)[course])
print 'Going to safe point 3'
yield boat.go_to_ecef_pos(
dict(
pool=[1220416.51743, -4965356.4575, 3799838.03177],
A=ll(36.802185, -76.191492), # from google earth
B=ll(36.801972, -76.191849), # from google earth
)[course])
print 'Going to safe point a'
yield boat.go_to_ecef_pos(
dict(
pool=[1220416.51743, -4965356.4575, 3799838.03177],
A=ll(36.801897, -76.191622), # from google earth
B=ll(36.801972, -76.191849), # from google earth
)[course])
# center far
#yield boat.go_to_ecef_pos(ll(36.802094, -76.191680))
yield boat.go_to_ecef_pos(ll(36.801694, -76.191034))
# center near
#yield boat.go_to_ecef_pos([1220440.29354, -4965392.18483, 3799791.58982])
yield boat.go_to_ecef_pos(
[1220451.80321, -4965388.40181, 3799791.9771])
yield boat.move.heading_deg(dict(A=60 + 90 + 180, )[course]).go()
fwd_task = boat.move.forward(100).go(speed=.2)
try:
yield boat.wait_for_bump()
finally:
fwd_task.cancel()
try:
x = boat.float()
yield util.sleep(1e6)
finally:
x.cancel()
print 'main end'
finally:
print 'main finally start'
#yield util.sleep(3)
print 'main finally end'
def main(nh, shape=None, color=None):
if shape == None:
shape = DEFAULT_SHAPE
if color == None:
color = DEFAULT_COLOR
boat = yield boat_scripting.get_boat(nh, False, False)
reorient = True
# While the boat is still distant from the target
while True and not rospy.is_shutdown():
temp_distance = 0
avg_distance = 0
shortest_distance = 100
farthest_distance = 0
x_mean = 0
y_mean = 0
numerator = 0
denom = 0
try:
yield util.wrap_timeout(center_sign(boat, shape, color), 20)
print "Locked onto shape"
except Exception:
print "Could not dock find shape, moving on to dock"
finally:
pass
# Only reorients one time
'''
if reorient == True and REORIENT == True:
print "reorienting"
orientation = yield orient(boat)
sign_centered = yield center_sign(boat, shape, color)
reorient = False
'''
print "holding at current position"
print "Scanning lidar"
distances = yield boat.get_distance_from_object(.05)
avg_distance = distances[0] - BOAT_LENGTH_OFFSET
shortest_distance = distances[1] - BOAT_LENGTH_OFFSET
farthest_distance = distances[2] - BOAT_LENGTH_OFFSET
print "Average distance from target:", avg_distance
print "Shortest distance between boat and object:", shortest_distance
print "Farther distance between boat and object:", farthest_distance
final_move = shortest_distance / MOVE_OFFSET
if farthest_distance > 3.5:
#raw_input("Press enter to move forward" + str(final_move) + " meters")
# Print only if a move is commanded
print "Moving forward " + str(final_move) + " meters"
yield boat.move.forward(final_move).go()
if farthest_distance <= 1.5:
#raw_input("Press enter to move forward one meter")
# Print only if a move is commanded
print "Moving forward one meter"
boat.move.forward(1.5).go()
print "Moving back 5 meters"
yield util.sleep(5)
yield boat.move.forward(-5).go()
print 'Find shape success'
break
# delete variables to avoid any threading problems
del avg_distance, temp_distance, farthest_distance, shortest_distance
def main(nh, shape=None, color=None):
if shape == None:
shape = DEFAULT_SHAPE
if color == None:
color = DEFAULT_COLOR
boat = yield boat_scripting.get_boat(nh, False, False)
reorient = True
# While the boat is still distant from the target
while True and not rospy.is_shutdown():
temp_distance = 0
avg_distance = 0
shortest_distance = 100
farthest_distance = 0
x_mean = 0
y_mean = 0
numerator = 0
denom = 0
try:
yield util.wrap_timeout(center_sign(boat, shape, color), 20)
print "Locked onto shape"
except Exception:
print "Could not dock find shape, moving on to dock"
finally:
pass
# Only reorients one time
"""
if reorient == True and REORIENT == True:
print "reorienting"
orientation = yield orient(boat)
sign_centered = yield center_sign(boat, shape, color)
reorient = False
"""
print "holding at current position"
print "Scanning lidar"
distances = yield boat.get_distance_from_object(0.05)
avg_distance = distances[0] - BOAT_LENGTH_OFFSET
shortest_distance = distances[1] - BOAT_LENGTH_OFFSET
farthest_distance = distances[2] - BOAT_LENGTH_OFFSET
print "Average distance from target:", avg_distance
print "Shortest distance between boat and object:", shortest_distance
print "Farther distance between boat and object:", farthest_distance
final_move = shortest_distance / MOVE_OFFSET
if farthest_distance > 3.5:
# raw_input("Press enter to move forward" + str(final_move) + " meters")
# Print only if a move is commanded
print "Moving forward " + str(final_move) + " meters"
yield boat.move.forward(final_move).go()
if farthest_distance <= 1.5:
# raw_input("Press enter to move forward one meter")
# Print only if a move is commanded
print "Moving forward one meter"
boat.move.forward(1.5).go()
print "Moving back 5 meters"
yield util.sleep(5)
yield boat.move.forward(-5).go()
print "Find shape success"
break
# delete variables to avoid any threading problems
del avg_distance, temp_distance, farthest_distance, shortest_distance
def main(nh):
while not rospy.is_shutdown():
boat = yield boat_scripting.get_boat(nh)
#boat.pan_lidar(min_angle=2.7, max_angle=3.14, freq=.75)
#boat.float_on()
#gate_viz_pub = rospy.Publisher('gates_viz', Marker, queue_size = 10)
#rospy.init_node("sdfhjhsdfjhgsdf")
angle_to_move = 0
hold = []
x_mean = 0
y_mean = 0
numerator = 0
denom = 0
pointcloud_base = []
# loops until lidar gets a good filtered lidar reading
while len(hold) <= 0:
pointcloud = yield boat.get_pointcloud()
yield util.sleep(.1) # sleep to avoid tooPast errors
pointcloud_base = yield boat.to_baselink(pointcloud)
yield util.sleep(.1) # sleep to avoid tooPast errors
pointcloud_enu = []
for p in pc2.read_points(pointcloud, field_names=("x", "y", "z"), skip_nans=False, uvs=[]):
pointcloud_enu.append((p[0], p[1], p[2]))
yield util.sleep(.1) # sleep to avoid tooPast errors
point_in_base = []
hold = []
# Filter lidar data to only data right in front of the boat
print "Filtering lidar data"
for i in range(0, len(pointcloud_base)):
temp = pointcloud_base[i]
if abs(temp[1]) < .3:
hold.append(pointcloud_enu[i])
for i in hold:
x_mean += i[0]
y_mean += i[1]
y_mean = y_mean/len(hold)
x_mean = x_mean/len(hold)
for i in hold:
numerator += (i[0] - x_mean)*(i[1] - y_mean)
denom += (i[0] - x_mean)*(i[0] - x_mean)
if denom == 0: denom = 1
m = numerator/denom
b = y_mean - (m * x_mean)
x1 = 1
x2 = 100
par_vect = m*1 + b
point2 = m*x2 + b
position = boat.odom.position[0:2]
yaw = quat_to_rotvec(boat.odom.orientation)[2]
heading = numpy.array([numpy.cos(yaw), numpy.sin(yaw)])
# Line we want to be orthagonal to
position1 = to_vector(x1, par_vect)
position3 = to_vector((heading[0]) + boat.odom.position[0], (heading[1]) + boat.odom.position[1])
position4 = to_vector((heading[0]+ .01) + boat.odom.position[0], (heading[1]+.1) + boat.odom.position[1])
#Vector to be parrallel to
vec1 = numpy.array([x1, par_vect, 0])
vec2 = ([0,0,0])
angle_to_move = 2
move = 0
angle_to_move = 1
#def find_angle():
# Calculte angle_to_move and position to move to get in front of object
position = boat.odom.position[0:2]
yaw = quat_to_rotvec(boat.odom.orientation)[2]
heading = numpy.array([numpy.cos(yaw), numpy.sin(yaw)])
vec2 = numpy.array([(heading[0]), (heading[1]), 0])
numerator = numpy.dot(vec1, vec2)
one = numpy.linalg.norm(vec1)
two = numpy.linalg.norm(vec2)
cosine = numerator / (one * two)
angle_to_move = math.acos(cosine)
#find_angle()
distances = yield boat.get_distance_from_object(.05)
theda_one = .9 - angle_to_move
L1 = distances[0]
distance_to_move = math.sin(theda_one) * L1
veccc = to_vector(x1,par_vect)
vecc = to_vector(x2, point2)
'''
m = Marker()
m.header = Header(frame_id = '/enu', stamp = rospy.Time.now())
m.ns = 'gates'
m.id = 1
m.type = Marker.LINE_STRIP
m.action = Marker.ADD
m.scale.x = 0.1
m.color.r = 0.5
m.color.b = 0.75
m.color.g = 0.1
m.color.a = 1.0
m.points.append(veccc)
m.points.append(vecc)
gate_viz_pub.publish(m)
'''
if vec2[0] < 0:
print "Yawing left ", angle_to_move
print "Moving right ", distance_to_move
raw_input("Press enter to continue...")
boat.move.turn_left(angle_to_move).go()
yield boat.move.left(-distance_to_move-2).go()
if vec2[0] > 0:
print "Yawing right ", angle_to_move
print "Moving left ", distance_to_move
raw_input("Press enter to continue...")
boat.move.turn_left(angle_to_move).go()
yield boat.move.left(distance_to_move-2).go()
def main(nh):
# #Print mission start msg
#print 'Finding start gate with laser'
boat = yield boat_scripting.get_boat(nh)
try:
yaw = quat_to_rotvec(boat.odom.orientation)[2]
#print 'Pan the lidar between the maximum angle and slightly above horizontal'
boat.pan_lidar(max_angle=3.264, min_angle=3.15, freq=0.5)
# How many gates we've gone through
gates_passed = 0
have_gate = False
last_gate_pos = None
move = None
while gates_passed < 2:
#print 'Get gates'
gates = yield boat.get_gates()
(gate_pos, gate_orientation) = (None, None)
if gates_passed == 0:
(gate_pos,
gate_orientation) = filter_gates(boat, gates, yaw, False)
else:
(gate_pos,
gate_orientation) = filter_gates(boat, gates, yaw, True)
# Check if valid gate found
if gate_pos is not None:
have_gate = True
# Check if we previously found a gate
if last_gate_pos is not None:
# Check if the gate center has drifted
# Don't go to a different gate (dis < 5)
dis = numpy.linalg.norm(last_gate_pos - gate_pos)
#print 'Distance: ', dis
if dis >= 0.1 and dis < 3:
print 'Gate drifted re-placing goal point'
#move = boat.move.set_position(gate_pos).set_orientation(gate_orientation).go()
if gates_passed == 0:
move = move_on_line.main(nh, gate_pos,
gate_orientation,
-GATE_DISTANCE)
else:
move = move_on_line.main(nh, gate_pos,
gate_orientation,
GATE_DISTANCE)
else:
#print 'Still happy with my current goal'
pass
else:
# Just found a gate
print 'Moving to gate ' + str(gates_passed + 1)
#move = boat.move.set_position(gate_pos).set_orientation(gate_orientation).go()
if gates_passed == 0:
#yield boat.move.yaw_left_deg(30).go()
move = move_on_line.main(nh, gate_pos,
gate_orientation,
-GATE_DISTANCE)
print 'zzz'
yield util.sleep(1.0)
else:
move = move_on_line.main(nh, gate_pos,
gate_orientation,
GATE_DISTANCE)
# Set last gate pos
last_gate_pos = gate_pos
else:
if have_gate is False:
print 'No gate found moving forward 1m'
yield boat.move.forward(1).go()
else:
print 'Lost sight of gate; Continuing to last known position'
# Check if task complete
if have_gate and move.called:
print 'Move complete'
#yield boat.move.forward(3).go()
yaw = quat_to_rotvec(boat.odom.orientation)[2]
have_gate = False
last_gate_pos = None
gates_passed = gates_passed + 1
finally:
boat.default_state()
def main(nh):
# #Print mission start msg
#print 'Finding start gate with laser'
boat = yield boat_scripting.get_boat(nh)
try:
yaw = quat_to_rotvec(boat.odom.orientation)[2]
#print 'Pan the lidar between the maximum angle and slightly above horizontal'
boat.pan_lidar(max_angle=3.264, min_angle=3.15, freq=0.5)
# How many gates we've gone through
gates_passed = 0
have_gate = False
last_gate_pos = None
move = None
while gates_passed < 2:
#print 'Get gates'
gates = yield boat.get_gates()
(gate_pos, gate_orientation) = (None, None)
if gates_passed == 0:
(gate_pos, gate_orientation) = filter_gates(boat, gates, yaw, False)
else:
(gate_pos, gate_orientation) = filter_gates(boat, gates, yaw, True)
# Check if valid gate found
if gate_pos is not None:
have_gate = True
# Check if we previously found a gate
if last_gate_pos is not None:
# Check if the gate center has drifted
# Don't go to a different gate (dis < 5)
dis = numpy.linalg.norm(last_gate_pos - gate_pos)
#print 'Distance: ', dis
if dis >= 0.1 and dis < 3:
print 'Gate drifted re-placing goal point'
#move = boat.move.set_position(gate_pos).set_orientation(gate_orientation).go()
if gates_passed == 0:
move = move_on_line.main(nh, gate_pos, gate_orientation, -GATE_DISTANCE)
else:
move = move_on_line.main(nh, gate_pos, gate_orientation, GATE_DISTANCE)
else:
#print 'Still happy with my current goal'
pass
else:
# Just found a gate
print 'Moving to gate ' + str(gates_passed + 1)
#move = boat.move.set_position(gate_pos).set_orientation(gate_orientation).go()
if gates_passed == 0:
#yield boat.move.yaw_left_deg(30).go()
move = move_on_line.main(nh, gate_pos, gate_orientation, -GATE_DISTANCE)
print 'zzz'
yield util.sleep(1.0)
else:
move = move_on_line.main(nh, gate_pos, gate_orientation, GATE_DISTANCE)
# Set last gate pos
last_gate_pos = gate_pos
else:
if have_gate is False:
print 'No gate found moving forward 1m'
yield boat.move.forward(1).go()
else:
print 'Lost sight of gate; Continuing to last known position'
# Check if task complete
if have_gate and move.called:
print 'Move complete'
#yield boat.move.forward(3).go()
yaw = quat_to_rotvec(boat.odom.orientation)[2]
have_gate = False
last_gate_pos = None
gates_passed = gates_passed + 1
finally:
boat.default_state()
def orient(boat):
angle_to_move = 0
final_cloud = []
x_mean = 0
y_mean = 0
numerator = 0
denom = 0
pointcloud_base = []
# loops until lidar gets a good filtered lidar reading
while len(final_cloud) <= 0:
pointcloud = yield boat.get_pointcloud()
yield util.sleep(.1) # sleep to avoid tooPast errors
pointcloud_base = yield boat.to_baselink(pointcloud)
yield util.sleep(.1) # sleep to avoid tooPast errors
pointcloud_enu = []
# Append pointcloud data to enu
for p in pc2.read_points(pointcloud,
field_names=("x", "y", "z"),
skip_nans=False,
uvs=[]):
pointcloud_enu.append((p[0], p[1], p[2]))
yield util.sleep(.1) # sleep to avoid tooPast errors
final_cloud = []
# Filter lidar data to only data right in front of the boat
# Since both pointcloud have the sane index for the same points,
# we then use the baselink pointcloud data to fiter enu points in front of the boat
print "Filtering lidar data"
for i in range(0, len(pointcloud_base)):
temp = pointcloud_base[i]
if abs(temp[1]) < .3:
final_cloud.append(pointcloud_enu[i])
# Use final point cloud to get x and y mean to be used in line generation
for i in final_cloud:
x_mean += i[0]
y_mean += i[1]
# Find x-mean and y-mean
y_mean = y_mean / len(final_cloud)
x_mean = x_mean / len(final_cloud)
for i in final_cloud:
numerator += (i[0] - x_mean) * (i[1] - y_mean)
denom += (i[0] - x_mean) * (i[0] - x_mean)
if denom == 0: denom = 1
m = numerator / denom
b = y_mean - (m * x_mean)
x1 = 1
x2 = 100
# Vector we want to be parallel to
parallel_vector = m * 1 + b
# Gets heading of the boat as unit vector
position = boat.odom.position[0:2]
yaw = quat_to_rotvec(boat.odom.orientation)[2]
heading = numpy.array([numpy.cos(yaw), numpy.sin(yaw)])
# Vector to be parrallel to
vec1 = numpy.array([x1, parallel_vector, 0])
# Vector of the boat
vec2 = numpy.array([(heading[0]), (heading[1]), 0])
angle_to_move = 2
move = 0
angle_to_move = 1
# Finds angle between boat unit vector and dock
numerator = numpy.dot(vec1, vec2)
one = numpy.linalg.norm(vec1)
two = numpy.linalg.norm(vec2)
cosine = numerator / (one * two)
# Angle betwen the boat and the shore
angle_to_move = math.acos(cosine)
# Scan lidar to get the distances from the shore to the boat for use in creating triangle
distances = yield boat.get_distance_from_object(.05)
# Caluculate distance to move to be directly in front of point
theda_one = .9 - angle_to_move
L1 = distances[0]
distance_to_move = math.sin(theda_one) * L1
if vec2[0] < 0:
#print "Yawing left ", angle_to_move
print "Moving right ", distance_to_move
#boat.move.turn_left(angle_to_move).go()
left = boat.move.left(-distance_to_move - 2).go()
forward = boat.move.forward(distance_to_move - 3).go()
yield left, forward
if vec2[0] > 0:
#print "Yawing right ", angle_to_move
print "Moving left ", distance_to_move
#boat.move.turn_left(angle_to_move).go()
left = boat.move.left(distance_to_move - 2).go()
forward = boat.move.forward(distance_to_move - 3).go()
yield left, forward
def try_to_grab(sub, obj_name, freq, surface=False, bubbles=False):
assert obj_name in ['moonrock', 'cheese']
try:
yield sub.move.depth(0.4).go()
fwd_move = sub.move.go(linear=[0.25, 0, 0])
try:
yield sub.visual_align('down', 'wreath/board/high', 2, selector=select_centered, turn=True, angle=math.radians(45))
finally:
yield fwd_move.cancel()
board_pose = sub.pose.depth(1).right(.6)
yield sub.move.yaw_left_deg(90*random.randrange(4)).go()
yield sub.move.depth(2).go()
try:
yield util.wrap_timeout(sub.visual_align('down', 'wreath/moonrock/high', 2, selector=selector(obj_name), one_shot=True, turn=False), 20)
except util.TimeoutError:
print 'timed out'
yield sub.move.yaw_left_deg(120).go()
return
try:
yield util.wrap_timeout(sub.visual_align('down', 'wreath/moonrock/high', 2, selector=select_centered), 20)
except util.TimeoutError:
print 'timed out 1'
yield sub.move.yaw_left_deg(120).go()
return
#print "weighing"
#w1 = yield get_weight(sub)
#print 'w1', w1
print "moving down"
yield sub.move.down(1).go(speed=.2)
try:
yield util.wrap_timeout(sub.visual_align('down', 'wreath/moonrock/low', 1, selector=select_centered, turn=False), 20)
except util.TimeoutError:
print 'timed out 2'
return
yield sub.lower_down_grabber()
yield sub.open_down_grabber()
yield sub.move.relative([-.10 if obj_name == 'moonrock' else -.115,-.15,0]).go()
if obj_name == 'moonrock':
yield sub.move.down(.73).go(speed=.2)
else:
yield sub.move.down(.81).go(speed=.2)
yield sub.close_down_grabber()
print "moving back to surface"
#yield sub.move.up(.5).go(speed=.2)
yield sub.move.depth(1).go()
#w2 = yield get_weight(sub)
#print 'w2', w2
#gain = w2 - w1
#print 'weight gained', gain
#weights = {0: 0, 1: 2.97, 2: 4.00}
#objects = min(weights, key=lambda w: abs(weights[w] - gain))
#print 'object count', objects
#if objects != 1:
# yield sub.move.set_position(board_pose.position + [random.uniform(-.5, .5), random.uniform(-.5, .5), 0]).go()
# yield sub.open_down_grabber()
# yield util.sleep(1)
# yield sub.close_down_grabber()
# yield sub.move.turn_left_deg(120).go()
# defer.returnValue(False)
print "going to hydrophone"
yield sub.hydrophone_align(freq)
yield sub.move.relative(RELATIVE_PINGER_MOVE).go()
yield sub.move.depth(.4).go()
yield sub.raise_down_grabber()
bin_pose = sub.move
try:
yield util.wrap_timeout(sub.visual_align('down', 'wreath/bin/high', 2, selector=select_centered, turn=False), 10)
yield sub.move.relative(RELATIVE_VISION_MOVE).go()
except util.TimeoutError:
print 'bin alignment timed out'
yield bin_pose.go()
except:
print 'bin alignment???'
yield bin_pose.go()
if surface:
yield sub.move.depth(0).go()
yield sub.move.depth(.4).go()
yield sub.lower_down_grabber()
#yield sub.move.relative([-.15,-.2,0]).go()
yield sub.move.depth(2).go()
print "going down"
yield sub.open_down_grabber()
yield util.sleep(3)
yield sub.close_down_grabber()
yield util.sleep(1)
yield sub.open_down_grabber()
yield util.sleep(3)
yield sub.raise_down_grabber()
if bubbles:
yield sub.fire_left_torpedo()
yield sub.fire_right_torpedo()
yield sub.fire_left_torpedo()
yield sub.fire_right_torpedo()
yield sub.fire_left_torpedo()
yield sub.fire_right_torpedo()
print "going to board"
yield sub.move.look_at_without_pitching(board_pose.position).go()
yield sub.move.set_position(board_pose.position).go()
finally:
print "finally"
yield sub.close_down_grabber()
yield sub.raise_down_grabber()
#yield sub.move.depth(2).go()
defer.returnValue(True)
def hold_at_current_pos(self):
move = self.move.set_position(self.odom.position).go()
yield util.sleep(3)
move.cancel()
def raise_impaler(self):
yield self._set_valve_service(1, False)
yield util.sleep(0.1)
yield self._set_valve_service(0, True)
def lower_impaler(self):
yield self._set_valve_service(0, False)
yield util.sleep(0.1)
yield self._set_valve_service(1, True)
def close_gripper(self):
yield self._set_valve_service(SetValveRequest(valve=1, opened=False))
yield util.sleep(.3)
yield self._set_valve_service(SetValveRequest(valve=3, opened=True))
def orient(boat):
angle_to_move = 0
final_cloud = []
x_mean = 0
y_mean = 0
numerator = 0
denom = 0
pointcloud_base = []
# loops until lidar gets a good filtered lidar reading
while len(final_cloud) <= 0:
pointcloud = yield boat.get_pointcloud()
yield util.sleep(0.1) # sleep to avoid tooPast errors
pointcloud_base = yield boat.to_baselink(pointcloud)
yield util.sleep(0.1) # sleep to avoid tooPast errors
pointcloud_enu = []
# Append pointcloud data to enu
for p in pc2.read_points(pointcloud, field_names=("x", "y", "z"), skip_nans=False, uvs=[]):
pointcloud_enu.append((p[0], p[1], p[2]))
yield util.sleep(0.1) # sleep to avoid tooPast errors
final_cloud = []
# Filter lidar data to only data right in front of the boat
# Since both pointcloud have the sane index for the same points,
# we then use the baselink pointcloud data to fiter enu points in front of the boat
print "Filtering lidar data"
for i in range(0, len(pointcloud_base)):
temp = pointcloud_base[i]
if abs(temp[1]) < 0.3:
final_cloud.append(pointcloud_enu[i])
# Use final point cloud to get x and y mean to be used in line generation
for i in final_cloud:
x_mean += i[0]
y_mean += i[1]
# Find x-mean and y-mean
y_mean = y_mean / len(final_cloud)
x_mean = x_mean / len(final_cloud)
for i in final_cloud:
numerator += (i[0] - x_mean) * (i[1] - y_mean)
denom += (i[0] - x_mean) * (i[0] - x_mean)
if denom == 0:
denom = 1
m = numerator / denom
b = y_mean - (m * x_mean)
x1 = 1
x2 = 100
# Vector we want to be parallel to
parallel_vector = m * 1 + b
# Gets heading of the boat as unit vector
position = boat.odom.position[0:2]
yaw = quat_to_rotvec(boat.odom.orientation)[2]
heading = numpy.array([numpy.cos(yaw), numpy.sin(yaw)])
# Vector to be parrallel to
vec1 = numpy.array([x1, parallel_vector, 0])
# Vector of the boat
vec2 = numpy.array([(heading[0]), (heading[1]), 0])
angle_to_move = 2
move = 0
angle_to_move = 1
# Finds angle between boat unit vector and dock
numerator = numpy.dot(vec1, vec2)
one = numpy.linalg.norm(vec1)
two = numpy.linalg.norm(vec2)
cosine = numerator / (one * two)
# Angle betwen the boat and the shore
angle_to_move = math.acos(cosine)
# Scan lidar to get the distances from the shore to the boat for use in creating triangle
distances = yield boat.get_distance_from_object(0.05)
# Caluculate distance to move to be directly in front of point
theda_one = 0.9 - angle_to_move
L1 = distances[0]
distance_to_move = math.sin(theda_one) * L1
if vec2[0] < 0:
# print "Yawing left ", angle_to_move
print "Moving right ", distance_to_move
# boat.move.turn_left(angle_to_move).go()
left = boat.move.left(-distance_to_move - 2).go()
forward = boat.move.forward(distance_to_move - 3).go()
yield left, forward
if vec2[0] > 0:
# print "Yawing right ", angle_to_move
print "Moving left ", distance_to_move
# boat.move.turn_left(angle_to_move).go()
left = boat.move.left(distance_to_move - 2).go()
forward = boat.move.forward(distance_to_move - 3).go()
yield left, forward
def set_thruster_thread():
while True:
pub.publish(ThrusterCommand(force=power_holder[0]))
yield util.sleep(.1)
