Untitled

#!/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