class Scale(object):
CALIBRATION_WEIGHT = 20
WAIT = 1000
def __init__(self):
self.channel = VoltageRatioInput()
self.channel.openWaitForAttachment(Scale.WAIT)
self.lower_ratio = None
self.higher_ratio = None
def wait_for_keypress(self, queue):
while queue.empty():
pass
while not queue.empty():
queue.get()
def calibrate(self, queue):
log.info("Beginning calibration.")
log.info("Please take off any weights from the scale and hit enter:")
self.wait_for_keypress(queue)
self.lower_ratio = self.measure_ratio()
log.info("Please put the %sg weight on scale and hit enter:" %
Scale.CALIBRATION_WEIGHT)
self.wait_for_keypress(queue)
self.higher_ratio = self.measure_ratio()
return self
def measure_ratio(self):
voltage_ratio = self.channel.getVoltageRatio()
return float(voltage_ratio)
def measure_weight(self):
ratio = self.measure_ratio()
return self.CALIBRATION_WEIGHT * (ratio - self.lower_ratio) / (
self.higher_ratio - self.lower_ratio)
def __init__(self,
bridge_channel,
topic,
frame_id,
bridge_serial = 572843,
bridge_hubport = 0,
bridge_hubportdevice = False,
data_rate_Hz = 60,
gain = BridgeGain.BRIDGE_GAIN_128,
force_conversion_multiple = 1.0,
force_conversion_offset = 0.0):
self.data_rate_Hz = data_rate_Hz
self.data_interval_ms = int((1.0/self.data_rate_Hz)*1000)
self.publishcount = 0
self.force_conversion_multiple = float(force_conversion_multiple)
self.force_conversion_offset = float(force_conversion_offset)
self.gain = gain
self.pb_ch = VoltageRatioInput()
self.pb_ch.setDeviceSerialNumber(bridge_serial)
self.pb_ch.setHubPort(bridge_hubport)
self.pb_ch.setIsHubPortDevice(bridge_hubportdevice)
self.pb_ch.setChannel(int(bridge_channel))
rospy.init_node("phidget_bridge_channel", anonymous=True)
self.pub = rospy.Publisher(topic, WrenchStamped, queue_size=10)
self.frame_id = frame_id
self.pb_ch.setOnAttachHandler(self.onAttachHandler)
self.pb_ch.setOnDetachHandler(self.onDetachHandler)
self.pb_ch.setOnVoltageRatioChangeHandler(self.onVoltageRatioChangeHandler)
def __init__(self, name: str, channel: int):
self.name = name
self.value = 0
self.valid = None
self.lock = threading.Lock()
self.phidget = VoltageRatioInput()
self.phidget.setChannel(channel)
self.phidget.setOnSensorChangeHandler(self._on_change)
self.phidget.setOnErrorHandler(self._on_error)
def __init__(self, serialNo, channel):
VoltageRatioInput.__init__(self)
self.setDeviceSerialNumber(serialNo)
self.setChannel(channel)
try:
self.openWaitForAttachment(10000)
except PhidgetException as e:
print("PhidgetException on open - code %i: %s" %
(e.code, e.details))
raise PhidgetException(e.code)
self.setDataInterval(self.getMinDataInterval())
self.setBridgeGain(BridgeGain.BRIDGE_GAIN_128)
self.setBridgeEnabled(True)
def __init__(self,
topic,
frame_id,
bridge_channel,
bridge_serial = None,
bridge_hubport= 0,
bridge_is_hubport_device = False,
data_rate_Hz = 60,
gain = BridgeGain.BRIDGE_GAIN_128,
force_conversion_multiple = 1.0,
force_conversion_offset = 0.0):
super().__init__('phidgetbridge_node')
self.topic = topic
self.frame_id = frame_id
self.pb_ch = VoltageRatioInput()
self.pb_ch.setChannel(int(bridge_channel))
self.pb_ch.setHubPort(bridge_hubport)
self.pb_ch.setIsHubPortDevice(bridge_is_hubport_device)
if bridge_serial is not None:
self.pb_ch.setDeviceSerialNumber(bridge_serial)
self.data_rate_Hz = data_rate_Hz
self.data_interval_ms = int((1.0/self.data_rate_Hz)*1000)
self.gain = gain
self.force_conversion_multiple = float(force_conversion_multiple)
self.force_conversion_offset = float(force_conversion_offset)
self.pub_force = self.create_publisher(WrenchStamped, self.topic, 1)
self.pb_ch.setOnAttachHandler(self.onAttachHandler)
self.pb_ch.setOnVoltageRatioChangeHandler(self.onVoltageRatioChangeHandler)
self.pb_ch.openWaitForAttachment(1000)
class ROSPhidgetBridgeChannel():
def __init__(self,
bridge_channel,
topic,
frame_id,
bridge_serial = 572843,
bridge_hubport = 0,
bridge_hubportdevice = False,
data_rate_Hz = 60,
gain = BridgeGain.BRIDGE_GAIN_128,
force_conversion_multiple = 1.0,
force_conversion_offset = 0.0):
self.data_rate_Hz = data_rate_Hz
self.data_interval_ms = int((1.0/self.data_rate_Hz)*1000)
self.publishcount = 0
self.force_conversion_multiple = float(force_conversion_multiple)
self.force_conversion_offset = float(force_conversion_offset)
self.gain = gain
self.pb_ch = VoltageRatioInput()
self.pb_ch.setDeviceSerialNumber(bridge_serial)
self.pb_ch.setHubPort(bridge_hubport)
self.pb_ch.setIsHubPortDevice(bridge_hubportdevice)
self.pb_ch.setChannel(int(bridge_channel))
rospy.init_node("phidget_bridge_channel", anonymous=True)
self.pub = rospy.Publisher(topic, WrenchStamped, queue_size=10)
self.frame_id = frame_id
self.pb_ch.setOnAttachHandler(self.onAttachHandler)
self.pb_ch.setOnDetachHandler(self.onDetachHandler)
self.pb_ch.setOnVoltageRatioChangeHandler(self.onVoltageRatioChangeHandler)
def start(self):
rospy.loginfo("phidget bridge channel node started")
rospy.loginfo("waiting to be attached")
self.pb_ch.openWaitForAttachment(1000)
def force(self, voltage_ratio):
return (voltage_ratio*self.force_conversion_multiple) + self.force_conversion_offset
def onAttachHandler(self, channel):
rospy.loginfo("phidget bridge serial "+str(channel.getDeviceSerialNumber())+" channel "+str(channel.getChannel())+" attached")
channel.setDataInterval(self.data_interval_ms)
channel.setVoltageRatioChangeTrigger(0.0)
channel.setBridgeGain(self.gain)
channel.setBridgeEnabled(True)
def onDetachHandler(self, channel):
rospy.loginfo("phidget bridge serial "+str(channel.getDeviceSerialNumber())+" channel "+str(channel.getChannel())+" detached")
def onVoltageRatioChangeHandler(self, channel, voltageRatio):
self.pub.publish(
WrenchStamped(
Header(
self.publishcount,
rospy.Time.now(),
self.frame_id),
Wrench(
Vector3(self.force(voltageRatio), 0, 0),
Vector3(0,0,0))))
self.publishcount += 1
def setup_phidget():
log.i("Setting up sensor")
voltageRatioInput0 = VoltageRatioInput()
voltageRatioInput0.setIsHubPortDevice(True)
voltageRatioInput0.setHubPort(0)
voltageRatioInput0.setOnSensorChangeHandler(onSensorChange)
voltageRatioInput0.openWaitForAttachment(5000)
voltageRatioInput0.setSensorType(VoltageRatioSensorType.SENSOR_TYPE_1120)
voltageRatioInput0.setDataInterval(1)
log.i("sensor ready")
return voltageRatioInput0
class PhidgetBridgeNode(Node):
def __init__(self,
topic,
frame_id,
bridge_channel,
bridge_serial = None,
bridge_hubport= 0,
bridge_is_hubport_device = False,
data_rate_Hz = 60,
gain = BridgeGain.BRIDGE_GAIN_128,
force_conversion_multiple = 1.0,
force_conversion_offset = 0.0):
super().__init__('phidgetbridge_node')
self.topic = topic
self.frame_id = frame_id
self.pb_ch = VoltageRatioInput()
self.pb_ch.setChannel(int(bridge_channel))
self.pb_ch.setHubPort(bridge_hubport)
self.pb_ch.setIsHubPortDevice(bridge_is_hubport_device)
if bridge_serial is not None:
self.pb_ch.setDeviceSerialNumber(bridge_serial)
self.data_rate_Hz = data_rate_Hz
self.data_interval_ms = int((1.0/self.data_rate_Hz)*1000)
self.gain = gain
self.force_conversion_multiple = float(force_conversion_multiple)
self.force_conversion_offset = float(force_conversion_offset)
self.pub_force = self.create_publisher(WrenchStamped, self.topic, 1)
self.pb_ch.setOnAttachHandler(self.onAttachHandler)
self.pb_ch.setOnVoltageRatioChangeHandler(self.onVoltageRatioChangeHandler)
self.pb_ch.openWaitForAttachment(1000)
def onAttachHandler(self, channel):
channel.setDataInterval(self.data_interval_ms)
channel.setVoltageRatioChangeTrigger(0.0)
channel.setBridgeGain(self.gain)
channel.setBridgeEnabled(True)
def voltageRatioToForce(self, voltage_ratio):
return ((voltage_ratio*self.force_conversion_multiple)
+ self.force_conversion_offset)
def onVoltageRatioChangeHandler(self, channel, voltageRatio):
self.pub_force.publish(
WrenchStamped(
header=Header(
stamp=self.get_clock().now().to_msg(),
frame_id=self.frame_id),
wrench=Wrench(
force=Vector3(x=self.voltageRatioToForce(voltageRatio),
y=0.0,
z=0.0),
torque=Vector3(x=0.0, y=0.0, z=0.0))))
def __init__(self):
self.channel = VoltageRatioInput()
self.channel.openWaitForAttachment(Scale.WAIT)
self.lower_ratio = None
self.higher_ratio = None
def main():
"The main function: loads an input and waits until terminated."
try:
try:
channel = VoltageRatioInput()
except PhidgetException as e:
sys.stderr.write(
"Runtime Error -> Creating VoltageRatioInput: \n\t")
display_error(e)
raise
except RuntimeError as e:
sys.stderr.write(
"Runtime Error -> Creating VoltageRatioInput: \n\t" + e)
raise
channel.setChannel(0)
channel.setOnAttachHandler(on_attach)
channel.setOnErrorHandler(on_error)
channel.setOnVoltageRatioChangeHandler(on_voltage_change)
channel.setOnSensorChangeHandler(on_sensor_change)
print("\nOpening and Waiting for Attachment...")
try:
channel.openWaitForAttachment(5000)
# Set the sensor type to the 10-80cm distance one
channel.setSensorType(
VoltageRatioSensorType.SENSOR_TYPE_1101_SHARP_2Y0A21)
except PhidgetException as e:
print_open_error(e, channel)
return
print("Sampling data for 10 seconds.")
while True:
time.sleep(60)
return
except PhidgetException as e:
sys.stderr.write("\nExiting with error(s)...")
display_error(e)
traceback.print_exc()
return
finally:
print("Cleaning up...")
channel.setOnVoltageRatioChangeHandler(None)
channel.setOnSensorChangeHandler(None)
channel.close()
class Distance:
"""A glorified wrapper over the distance sensor."""
name = None # type: str
value = None # type: float
valid = None # type: Optional[bool]
def __init__(self, name: str, channel: int):
self.name = name
self.value = 0
self.valid = None
self.lock = threading.Lock()
self.phidget = VoltageRatioInput()
self.phidget.setChannel(channel)
self.phidget.setOnSensorChangeHandler(self._on_change)
self.phidget.setOnErrorHandler(self._on_error)
def _on_change(self, _, value, _unit):
"Callback for when the sensor's input is changed." ""
with self.lock:
if not self.valid or abs(self.value - value) > 0.05:
# Update properties and notify observers
if self.name == "front_dist_0" or self.name == "front_dist_1":
LOG.debug("%s = %s%s", self.name, value, _unit.symbol)
self.value = value
self.valid = True
def get(self) -> float:
""" Returns the value of the sensors data """
with self.lock:
data = self.value
return data
def get_valid(self) -> Optional[bool]:
""" Returns the true/false if the sensor reading is valid"""
with self.lock:
return self.valid
def _on_error(self, ph, code, msg):
"""Callback for when the sensor detects receives an error.
We have a special implementation, as we want to handle the case where
the input is out of range.
"""
if code == 4103:
# Mark as malformed and notify observers
with self.lock:
if self.valid or self.valid is None:
LOG.warning("%s is out of bounds", self.name)
self.valid = False
else:
on_error(ph, code, msg)
def __enter__(self):
"""Attach this sensor and configure it with various properties.
For now, we subscribe to updates every 50ms (20Hz).
"""
self.phidget.openWaitForAttachment(ATTACHMENT_TIMEOUT)
self.phidget.setDataInterval(50)
self.phidget.setSensorType(
VoltageRatioSensorType.SENSOR_TYPE_1101_SHARP_2D120X)
LOG.info("Attached %s", self.name)
return self
def __exit__(self, _a, _b, _c):
self.phidget.setOnErrorHandler(None)
self.phidget.setOnSensorChangeHandler(None)
self.phidget.close()