diff --git a/scripts/stream_node.py b/scripts/stream_node.py index c5620f8c17c2fa295a5ea84b88ce0bd54b072d41..1332a7b0c45790ef2bdb09d2fa76ef1c2ecd2c52 100755 --- a/scripts/stream_node.py +++ b/scripts/stream_node.py @@ -21,7 +21,7 @@ import ue9 class Labjack(object): - def __init__(self, channel, resolution=16, scan_freq=150): + def __init__(self, channel, resolution=16, scan_freq=366): self.channel_name = "AIN"+str(channel) self.d = ue9.UE9() self.d.getCalibrationData() @@ -39,21 +39,24 @@ class Labjack(object): # Anonymous topic ? self.pub = rospy.Publisher('chatter', Float32, queue_size=10) - timer_period = 0.01 # in second - self.timer = rospy.Timer(rospy.Duration(timer_period), self.timer_callback) - rospy.spin() + #timer_period = 0.01 # in second + #self.timer = rospy.Timer(rospy.Duration(timer_period), self.timer_callback) + #rospy.spin() + + while rospy.is_shutdown() is False: + self.timer_callback() self.d.streamStop() self.d.close() - def timer_callback(self, event): + def timer_callback(self): msg = Float32() # Conversion in milli volt for gain 1 - for data in self.d.streamData(): - if data is not None: - msg.data = sum(data[self.channel_name])/len(data[self.channel_name]) - rospy.loginfo('Publishing: "{0}"'.format(msg.data)) - self.pub.publish(msg) + data = next(self.d.streamData()) + if data is not None: + msg.data = sum(data[self.channel_name])/len(data[self.channel_name]) + rospy.loginfo('Publishing: "{0}"'.format(msg.data)) + self.pub.publish(msg) if __name__ == '__main__':