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