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 __init__(self, name, serial_no):
self.name = name
self.serial_no = serial_no
self.shutdown_token = Event()
self.logger = Logger(verbose=False)
self.device = None
odrive.find_all("usb", self.serial_no,
lambda device: self.did_discover_device(device),
self.shutdown_token, self.shutdown_token, self.logger)
# Get ODrives
done_signal = Event(None)
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)))
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()