(Jetson Project#7)Launch the MPU6050 IMU Node and Visualize in RViz (2)_Trouble Shooting

Following the previous post, I want to visualizing.

For this, Verify the IMU Node Functionality first.

After launching, you should verify that the IMU node is publishing data correctly.

4.1. Check ROS Topics

In a new terminal, ensure your ROS environment is sourced:

source /opt/ros/noetic/setup.bash
source ~/catkin_ws/devel/setup.bash

Then, list the active ROS topics:

rostopic list

Expected Output:

/imu/data_raw
/rosout
/rosout_agg

But, I can see only
/rosout
/rosout_agg

4.1. Check ROS Topics_ Trouble Shooting.

Step 1: Verify and Correct the mpu6050_publisher.py Script

1.1. Ensure All Necessary Constants Are Defined
The NameError indicates that ACCEL_CONFIG wasn't defined. Let's ensure that all necessary register constants for the MPU6050 are correctly defined in your script.

locate

nano ~/catkin_ws/src/mpu6050_driver/scripts/mpu6050_publisher.py

Corrected mpu6050_publisher.py Script:

#!/usr/bin/env python3
import rospy
import smbus
import time
from sensor_msgs.msg import Imu
from geometry_msgs.msg import Quaternion
from tf.transformations import quaternion_from_euler

# MPU6050 Registers
PWR_MGMT_1   = 0x6B
SMPLRT_DIV   = 0x19
CONFIG       = 0x1A
GYRO_CONFIG  = 0x1B
ACCEL_CONFIG = 0x1C  # Added definition for ACCEL_CONFIG
INT_ENABLE   = 0x38
ACCEL_XOUT_H = 0x3B
ACCEL_YOUT_H = 0x3D
ACCEL_ZOUT_H = 0x3F
GYRO_XOUT_H  = 0x43
GYRO_YOUT_H  = 0x45
GYRO_ZOUT_H  = 0x47

def read_word(bus, addr):
    high = bus.read_byte_data(0x68, addr)
    low = bus.read_byte_data(0x68, addr+1)
    val = (high << 8) + low
    if val >= 0x8000:
        return -((65535 - val) + 1)
    else:
        return val

def mpu6050_init(bus):
    # Wake up the MPU6050
    bus.write_byte_data(0x68, PWR_MGMT_1, 0)
    # Set accelerometer configuration (±2g)
    bus.write_byte_data(0x68, ACCEL_CONFIG, 0x00)
    # Set gyroscope configuration (±250°/s)
    bus.write_byte_data(0x68, GYRO_CONFIG, 0x00)

def main():
    rospy.init_node('mpu6050_publisher', anonymous=True)
    imu_pub = rospy.Publisher('/imu/data_raw', Imu, queue_size=10)
    bus = smbus.SMBus(1)  # I2C bus

    try:
        mpu6050_init(bus)
    except Exception as e:
        rospy.logerr(f"Failed to initialize MPU6050: {e}")
        rospy.signal_shutdown("MPU6050 initialization failed")

    imu_msg = Imu()
    imu_msg.header.frame_id = "imu_link"

    rate = rospy.Rate(10)  # 10 Hz

    while not rospy.is_shutdown():
        try:
            accel_x = read_word(bus, ACCEL_XOUT_H) / 16384.0
            accel_y = read_word(bus, ACCEL_YOUT_H) / 16384.0
            accel_z = read_word(bus, ACCEL_ZOUT_H) / 16384.0

            gyro_x = read_word(bus, GYRO_XOUT_H) / 131.0
            gyro_y = read_word(bus, GYRO_YOUT_H) / 131.0
            gyro_z = read_word(bus, GYRO_ZOUT_H) / 131.0
        except Exception as e:
            rospy.logerr(f"Error reading MPU6050 data: {e}")
            rate.sleep()
            continue

        # Populate the Imu message
        imu_msg.header.stamp = rospy.Time.now()
        imu_msg.linear_acceleration.x = accel_x
        imu_msg.linear_acceleration.y = accel_y
        imu_msg.linear_acceleration.z = accel_z

        # For simplicity, set orientation to zero quaternion
        quat = quaternion_from_euler(0, 0, 0)
        imu_msg.orientation = Quaternion(*quat)

        # Set angular velocity
        imu_msg.angular_velocity.x = gyro_x
        imu_msg.angular_velocity.y = gyro_y
        imu_msg.angular_velocity.z = gyro_z

        # Optionally, set covariance matrices (set to high uncertainty if not calibrated)
        imu_msg.orientation_covariance = [0.1, 0, 0, 0, 0.1, 0, 0, 0, 0.1]
        imu_msg.angular_velocity_covariance = [0.1, 0, 0, 0, 0.1, 0, 0, 0, 0.1]
        imu_msg.linear_acceleration_covariance = [0.1, 0, 0, 0, 0.1, 0, 0, 0, 0.1]

        imu_pub.publish(imu_msg)

        rospy.loginfo(f"Accel: X={accel_x:.2f} Y={accel_y:.2f} Z={accel_z:.2f} | Gyro: X={gyro_x:.2f} Y={gyro_y:.2f} Z={gyro_z:.2f}")
        rate.sleep()

if __name__ == "__main__":
    try:
        main()
    except rospy.ROSInterruptException:
        pass

1.2. Key Corrections and Enhancements
Defined ACCEL_CONFIG: Added the missing ACCEL_CONFIG = 0x1C to avoid the NameError.
Error Handling: Wrapped the initialization and data reading sections in try-except blocks to catch and log exceptions without crashing the node abruptly.
Covariance Matrices: Set default covariance values to indicate uncertainty. Adjust these values based on your calibration results for better accuracy.
Logging Improvements: Enhanced logging with formatted output for better readability.

Make the Script Executable

chmod +x ~/catkin_ws/src/mpu6050_driver/scripts/mpu6050_publisher.py

Step 2: Rebuild the Catkin Workspace and Source

After making changes to scripts and launch files, it's essential to rebuild the workspace and source the setup file to apply the changes.

1. Navigate to catkin Workspace && Build the workspace

cd ~/catkin_ws
catkin_make

2. Source the Setup File:

source devel/setup.bash

Step 3: Launch the MPU6050 IMU Node

Now, let's launch the MPU6050 node and verify its functionality.

1. Launch the Node:

roslaunch mpu6050_driver mpu6050.launch

Just for test.

2. Verify Node Status

(open new terminal with ROS environment is sourced):

rostopic list

=> same. it doesn't show the /imu/data_raw

Next post....