def grab():
# create publisher that pusblishes Bools to the grab_success topic
# consumed by scan and nav
pub = rospy.Publisher('grab_success', Bool, queue_size=10)
# initialize node and name it grab
rospy.init_node('grab', anonymous=False)
rospy.loginfo("entered grab state")
# grab subscribes to grab_signal topic (published by nav)
rospy.Subscriber("grab_signal", Bool, handle_grab_signal)
# crotch cam
rospy.Subscriber('crotch/image/image_raw', Image, handle_img)
while new_crotch_img is None: pass
# once an image is received, starts a HaarDetector and
# Color_Filter for sample detection
#Color_Filter.init(new_crotch_img)
#rospy.loginfo("Color Filter started")
#HaarDetector.initialize_precached()
#rospy.loginfo("Haar Detector started")
rate = rospy.Rate(10) # 10hz
global ready
# TODO: we may need to use the uniboard_communication node instead
u = uniboard.Uniboard("/dev/ttyUSB1")
u.arm_home()
while not rospy.is_shutdown():
if ready: # will become true when nav sends the grab_signal
grab_finished = False
grab_succ = False
num_tries = 0
while grab_finished == False:
rospy.loginfo("identifying sample")
xy = identify_sample()
rospy.loginfo("coords: {0}".format(xy))
if xy is None:
rospy.loginfo("sample could not be identified")
break
# rospy.loginfo("pick up attempt #{0}".format(num_tries))
# grab_succ = pick_up(u,xy)
# if (grab_succ) or (num_tries == MAX_TRIES):
# grab_finished = True
# logs result and publishes grab_succ on topic grab_success
rospy.loginfo("grab success" if grab_succ else "grab failure")
ready = False
pub.publish(grab_succ)
rate.sleep()
def tests():
rospy.init_node('tests', anonymous=False)
rospy.loginfo('node initialized')
rospy.Subscriber('crotch/image/image_raw', Image, handle_img)
rospy.Subscriber('stereo/left/image_rect_color', Image, handle_scan_left)
rospy.Subscriber('stereo/right/image_rect_color', Image, handle_scan_right)
#rate = rospy.Rate(10)
u = uniboard.Uniboard("/dev/ttyUSB1")
u.arm_home()
# comment out all but one test
#test_forward_until_scanned_easy(u, False)
#nav.move_til_sample(u, False)
test_pickup_easy(u)
def __init__(self, ub=None, uniboard_location="/dev/ttyUSB1"):
if ub is None:
ub = uniboard.Uniboard(uniboard_location)
self.uniboard_location = uniboard_location
self.ub = ub # Uniboard
self.axes = ["X", "Y", "Z", "A"] # Axes
self.x_boundry = 0.03 # X axis boundry
self.x_offest = 0.02 # X axis offset from center
self.y_boundry = 0.04 # y axis boundry
self.y_offest = 0.0 # y axis offest from center
self.x_center = (ub.arm_max("X") / 2.0) + self.x_offest # Center after offset
self.y_center = (ub.arm_max("Y") / 2.0) + self.y_offest # Center after offset
self.max_wait_time = 3 # Max wait time for axis movement
self.time_step = 0.1 # Time step for wait functions
self.activate_arm()
def vid():
rospy.init_node('vid', anonymous=False)
rospy.loginfo('node initialized')
rospy.Subscriber('crotch/image/image_raw', Image, handle_img)
rate = rospy.Rate(10)
u = uniboard.Uniboard("/dev/ttyUSB1")
u.arm_home()
# MOVE FORWARD X meters
u.motor_left(0.1)
u.motor_right(0.1)
time.sleep(9)
u.motor_left(0)
u.motor_right(0)
# PICK UP SAMPLE
if TEST_PIT:
# move arm out of the way of the camera
u.arm_target("X", 0)
u.arm_target("Y", u.arm_max("Y"))
while u.arm_should_be_moving("X") or u.arm_should_be_moving("Y"):
pass
time.sleep(1)
if new_crotch_img2 is not None:
curr_crotch_img = new_crotch_img2
rospy.loginfo("image success")
xy = grab.identify_easy_sample(curr_crotch_img)
rospy.loginfo("coordinates: " + str(xy))
if xy:
rospy.loginfo("a sample has been detected")
grab.pick_up_at(u, xy)
else:
rospy.loginfo("no sample detected")
else:
pick_up_center(u)
# MOVE BACKWARD X meters
u.motor_left(-0.1)
u.motor_right(-0.1)
time.sleep(8.9)
u.motor_left(0)
u.motor_right(0)
#!/usr/bin/env python
#Rover RC control program. Lets someone drive the rover using the
#remote control.
#Written 2015-2016 by Nick Ames <*****@*****.**>
import uniboard
import time
u = uniboard.Uniboard("/dev/ttyUSB1")
while True:
#TODO: Adjust channels to the correct RC channels
if u.rc_valid()[1] == True:
u.motor_left(-u.rc_value(2))
u.motor_right(-u.rc_value(3))
time.sleep(.01)
def __init__(self):
self.u = uniboard.Uniboard('/dev/ttyUSB1')
wheel_commands = [i * .01 for i in range(-101, 100)]
self.left = ScrollableList(wheel_commands, start=101)
self.right = ScrollableList(wheel_commands, start=101)