def find_any(path="usb", serial_number=None,
search_cancellation_token=None, channel_termination_token=None,
timeout=None, logger=Logger(verbose=False), find_multiple=False):
"""
Blocks until the first matching Fibre object is connected and then returns that object
"""
result = []
done_signal = Event(search_cancellation_token)
def did_discover_object(obj):
result.append(obj)
if find_multiple:
if len(result) >= int(find_multiple):
done_signal.set()
else:
done_signal.set()
find_all(path, serial_number, did_discover_object, done_signal, channel_termination_token, logger)
try:
done_signal.wait(timeout=timeout)
except TimeoutError:
if not find_multiple:
return None
finally:
done_signal.set() # terminate find_all
if find_multiple:
return result
else:
return result[0] if len(result) > 0 else None
def get_odrives(self):
self.odrvs = [None, None, None]
# Get ODrives
done_signal = Event(None)
def discovered_odrv(obj):
print("Found odrive with sn: {}".format(obj.serial_number))
if obj.serial_number in self.SERIAL_NUMS:
self.odrvs[self.SERIAL_NUMS.index(obj.serial_number)] = obj
print("ODrive is # {}".format(
self.SERIAL_NUMS.index(obj.serial_number)))
else:
print("ODrive sn not found in list. New ODrive?")
if not None in self.odrvs:
done_signal.set()
odrive.find_all("usb", None, discovered_odrv, done_signal, None,
Logger(verbose=False))
# Wait for ODrives
try:
done_signal.wait(timeout=120)
finally:
done_signal.set()
rospy.loginfo("Found ODrives")
def find_all_odrives(self,
path="usb",
serial_number=None,
search_cancellation_token=None,
channel_termination_token=None,
timeout=30,
logger=Logger(verbose=True)):
"""
Blocks until timeout
"""
result = []
done_signal = Event(search_cancellation_token)
self.drive_count = 0
def did_discover_object(obj):
result.append(obj)
self.drive_count += 1
odrive.find_all(path, serial_number, did_discover_object, done_signal,
channel_termination_token, logger)
try:
done_signal.wait(timeout=timeout)
except TimeoutError:
print("Timeouted")
finally:
done_signal.set() # terminate find_all
return result
def find_any(path="usb", serial_number=None,
search_cancellation_token=None, channel_termination_token=None,
timeout=None, logger=Logger(verbose=False)):
"""
Blocks until the first matching Fibre node is connected and then returns that node
"""
result = [ None ]
done_signal = Event(search_cancellation_token)
def did_discover_object(obj):
result[0] = obj
done_signal.set()
find_all(path, serial_number, did_discover_object, done_signal, channel_termination_token, logger)
try:
done_signal.wait(timeout=timeout)
finally:
done_signal.set() # terminate find_all
return result[0]
def discovered_odrv(obj):
print("Found odrive with sn: {}".format(obj.serial_number))
if obj.serial_number in SERIAL_NUMS:
odrvs[SERIAL_NUMS.index(obj.serial_number)] = obj
print("ODrive is # {}".format(SERIAL_NUMS.index(
obj.serial_number)))
else:
print("ODrive sn not found in list. New ODrive?")
if not None in odrvs:
done_signal.set()
odrive.find_all("usb", None, discovered_odrv, done_signal, None,
Logger(verbose=False))
# Wait for ODrives
try:
done_signal.wait(timeout=120)
finally:
done_signal.set()
# Which odrives
if args.which_odrive == None:
to_calib = odrvs
else:
to_calib = [odrvs[args.which_odrive]]
for odrv in to_calib:
odrv.config.brake_resistance = 0.5
print("Calibrating ODrive # {}".format(to_calib.index(odrv)))
if args.calib_both_axis:
odrv.axis0.watchdog_feed()
odrv.axis1.watchdog_feed()
def __init__(self, timeout):
# specify left, middle, and right ODrives
rospy.loginfo("Looking for ODrives...")
self.SERIAL_NUMS = [
35593293288011, # Left, 0
35623406809166, # Middle, 1
35563278839886] # Right, 2
self.odrvs = [
None,
None,
None]
# Get ODrives
done_signal = Event(None)
def discovered_odrv(obj):
print("Found odrive with sn: {}".format(obj.serial_number))
if obj.serial_number in self.SERIAL_NUMS:
self.odrvs[self.SERIAL_NUMS.index(obj.serial_number)] = obj
print("ODrive is # {}".format(self.SERIAL_NUMS.index(obj.serial_number)))
else:
print("ODrive sn not found in list. New ODrive?")
if not None in self.odrvs:
done_signal.set()
odrive.find_all("usb", None, discovered_odrv, done_signal, None, Logger(verbose=False))
# Wait for ODrives
try:
done_signal.wait(timeout=120)
finally:
done_signal.set()
# self.odrv0 = odrive.find_any()
# # odrv1 = odrive.find_any()
# # odrv2 = odrive.find_any()
rospy.loginfo("Found ODrives")
# Set left and right axis
self.leftAxes = [self.odrvs[0].axis0, self.odrvs[0].axis1, self.odrvs[1].axis1]
self.rightAxes = [self.odrvs[1].axis0, self.odrvs[2].axis0, self.odrvs[2].axis1]
self.axes = self.leftAxes + self.rightAxes
# Set axis state
rospy.logdebug("Enabling Watchdog")
for ax in (self.leftAxes + self.rightAxes):
ax.watchdog_feed()
ax.config.watchdog_timeout = 2
ax.encoder.config.ignore_illegal_hall_state = True
# Clear errors
for odrv in self.odrvs:
dump_errors(odrv, True)
for ax in (self.leftAxes + self.rightAxes):
ax.controller.vel_ramp_enable = True
ax.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL
ax.controller.config.control_mode = CTRL_MODE_VELOCITY_CONTROL
# Sub to topic
rospy.Subscriber('joy', Joy, self.vel_callback)
# Set first watchdog
self.timeout = timeout # log error if this many seconds occur between received messages
self.timer = rospy.Timer(rospy.Duration(self.timeout), self.watchdog_callback, oneshot=True)
self.watchdog_fired = False
# Init other variables
self.last_msg_time = 0
self.last_recv_time = 0
self.next_wd_feed_time = 0
rospy.loginfo("Ready for topic")
rospy.spin()
