How to log USB sensor data in rospy?
I use the following bash script
to establish a connection to a device and print strings of data read by it to the terminal. However, I'd like to be able to publish this data to a ROS topic
and so I've attempted to write a node in rospy
to do the equivalent of the bash script.
Bash Script:
#!/bin/bash
sudo -i <<-EOF
rmmod ftdi_sio
rmmod usbserial
modprobe ftdi-sio
echo -n VENDOR_ID PRODUCT_ID | sudo tee /sys/bus/usb-serial/drivers/ftdi_sio/new_id
stty -F /dev/ttyUSB0 9600 cs8 -cstopb -parenb
echo -e "SOME_STRING" > /dev/ttyUSB0
apt install screen
script /dev/null
screen /dev/ttyUSB0
EOF
rospy node:
#!/usr/bin/python3
import rospy
from std_msgs.msg import Int16
import serial
import subprocess
import os
def log_data():
pub = rospy.Publisher('data', Int16, queue_size=1)
rate = rospy.Rate(1.5)
while not rospy.is_shutdown():
data = Int16()
os.system("[ -d ""/dev/ttyUSB0"" ] && sudo chmod -R 777 /dev/ttyUSB0")
os.system("sudo chmod -R 777 /dev/ttyUSB0")
subprocess.run(["sudo", "-i",
"""rmmod ftdi_sio""",
"""rmmod usbserial""",
"""modprobe ftdi-sio""",
"""echo -n VENDOR_ID PRODUCT_ID | sudo tee /sys/bus/usb-serial/drivers/ftdi_sio/new_id"""])
serial_port = serial.Serial(port='/dev/ttyUSB0',
baudrate=9600,
bytesize=serial.EIGHTBITS,
stopbits=serial.STOPBITS_ONE,
parity=serial.PARITY_NONE)
serial_port.write("SOME_STRING")
data.data = serial_port.read()
serial_port.close()
rospy.loginfo("Data String: " + str(data.data))
pub.publish(data)
rate.sleep()
if __name__ == '__main__':
try:
rospy.init_node('data_logger', anonymous=True)
log_data()
except rospy.ROSInterruptException:
pass
The output I print to terminal from the rospy node is a stream of lines like [INFO] [1621344997.745772]: Data String: b'\r'
and not the expected string of data values, which should have a format similar to A: XXXXX, B: XXXXX
. When I print the serial_port
I get this output:
Serial<id=0x7fb9850d2970, open=True>(port='/dev/ttyUSB0', baudrate=9600, bytesize=8, parity='N', stopbits=1, timeout=None, xonxoff=False, rtscts=False, dsrdtr=False)
Can anyone point out where I'm going wrong? I'm particularly unsure whether I have set my device to be accessible correctly from ttyUSB0
in the subprocess.run([... ...])
commands. Any ideas?