Untitled
unknown
plain_text
5 months ago
2.1 kB
3
Indexable
#!/usr/bin/env python import rospy from sensor_msgs.msg import Imu import serial def parse_imu_data(data): """Parse CSV string data to float values and return an Imu message""" imu_msg = Imu() try: # Split the CSV data string into values values = data.strip().split(',') # Parse each value from the CSV ax, ay, az = float(values[0]), float(values[1]), float(values[2]) gx, gy, gz = float(values[3]), float(values[4]), float(values[5]) # Populate the IMU message with accelerometer and gyroscope data imu_msg.linear_acceleration.x = ax imu_msg.linear_acceleration.y = ay imu_msg.linear_acceleration.z = az imu_msg.angular_velocity.x = gx imu_msg.angular_velocity.y = gy imu_msg.angular_velocity.z = gz except (IndexError, ValueError): rospy.logwarn("Received malformed IMU data: {}".format(data)) return imu_msg def imu_serial_publisher(): rospy.init_node('imu_serial_publisher') imu_pub = rospy.Publisher('/imu', Imu, queue_size=10) rate = rospy.Rate(10) # Set publishing rate (e.g., 10 Hz) # Configure the serial port connection to the Arduino ser = serial.Serial('/dev/ttyUSB0', 115200, timeout=1) # Use your actual serial port while not rospy.is_shutdown(): if ser.in_waiting > 0: # Read a line of CSV data from the serial port line = ser.readline().decode('utf-8').strip() # Parse and publish IMU data imu_msg = parse_imu_data(line) imu_msg.header.stamp = rospy.Time.now() imu_msg.header.frame_id = "imu_link" # Set frame ID as appropriate # Publish the IMU data to the /imu topic imu_pub.publish(imu_msg) rospy.loginfo("Published IMU data: {}".format(imu_msg)) rate.sleep() if __name__ == '__main__': try: imu_serial_publisher() except rospy.ROSInterruptException: pass
Editor is loading...
Leave a Comment