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 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
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))))