(Jetson Project#6)MPU6050 GY-521 6-Axis IMU Sensor_Custom Script Create

Part 3: Install Necessary Software Libraries

To interact with the MPU6050 via I2C in Python, install the required libraries.

3.1. Install Python Libraries

1. Install smbus and i2c-tools:

sudo apt-get update
sudo apt-get install -y python3-smbus
sudo apt-get install -y i2c-tools

2. Install rospy and ROS Dependencies:

Ensure your ROS environment is properly sourced.

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

Install any missing dependencies using rosdep:

cd ~/catkin_ws/

rosdep install --from-paths src --ignore-src -r -y

3.2. Verify Python I2C Communication

Before integrating with ROS, test if you can communicate with the MPU6050.

1. Create a Python Script to Read MPU6050 Data:

mkdir -p ~/catkin_ws/src/mpu6050_driver/scripts
nano ~/catkin_ws/src/mpu6050_driver/scripts/mpu6050_test.py

2.Add the Following Content:

#!/usr/bin/env python3
import smbus
import time

# MPU6050 Registers
PWR_MGMT_1   = 0x6B
SMPLRT_DIV   = 0x19
CONFIG       = 0x1A
GYRO_CONFIG  = 0x1B
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 main():
    bus = smbus.SMBus(1)  # I2C bus
    # Wake up the MPU6050
    bus.write_byte_data(0x68, PWR_MGMT_1, 0)

    while True:
        accel_x = read_word(bus, ACCEL_XOUT_H)
        accel_y = read_word(bus, ACCEL_YOUT_H)
        accel_z = read_word(bus, ACCEL_ZOUT_H)

        gyro_x = read_word(bus, GYRO_XOUT_H)
        gyro_y = read_word(bus, GYRO_YOUT_H)
        gyro_z = read_word(bus, GYRO_ZOUT_H)

        print(f"Accel: X={accel_x} Y={accel_y} Z={accel_z} | Gyro: X={gyro_x} Y={gyro_y} Z={gyro_z}")
        time.sleep(1)

if __name__ == "__main__":
    main()

3. Make the Script Executable:

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

4. Run the Test Script:

sudo python3 ~/catkin_ws/src/mpu6050_driver/scripts/mpu6050_test.py

Part 4: Create a Custom ROS Package for MPU6050

Now, we'll create a ROS package that reads data from the MPU6050 and publishes it as sensor_msgs/Imu.

4.1. Create the ROS Package

1. Navigate to Your Catkin Workspace's Source Directory:

cd ~/catkin_ws/src

2. Create the Package:

catkin_create_pkg mpu6050_driver rospy std_msgs sensor_msgs tf

4.2. Develop the IMU Publisher Node

1. Navigate to the Package's Scripts Directory:

mkdir -p ~/catkin_ws/src/mpu6050_driver/scripts
cd ~/catkin_ws/src/mpu6050_driver/scripts

2. Create the mpu6050_publisher.py Script:

nano mpu6050_publisher.py

3. Add the Following Content:

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

    mpu6050_init(bus)

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

    rate = rospy.Rate(10)  # 10 Hz

    while not rospy.is_shutdown():
        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

        # 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
        imu_msg.orientation_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
        imu_msg.angular_velocity_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
        imu_msg.linear_acceleration_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]

        imu_pub.publish(imu_msg)

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

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

Initialization: Wakes up the MPU6050 and configures the accelerometer and gyroscope ranges.
Data Reading: Reads accelerometer and gyroscope data from the MPU6050.
Converts raw data to physical units (g for acceleration and °/s for gyroscope).
Publishing:Populates a sensor_msgs/Imu message with the read data.
Publishes the message to the /imu/data_raw topic.
Logging: Logs the acceleration and gyroscope data to the console for verification.

4. Make the Script Executable:

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

4.3. Update package.xml and CMakeLists.txt

1. Edit package.xml:

nano ~/catkin_ws/src/mpu6050_driver/package.xml

Ensure the Following Dependencies Are Listed:

<depend>rospy</depend>
<depend>std_msgs</depend>
<depend>sensor_msgs</depend>
<depend>tf</depend>

2. Edit CMakeLists.txt:

nano ~/catkin_ws/src/mpu6050_driver/CMakeLists.txt

Add the Following Lines at the End:

catkin_install_python(PROGRAMS scripts/mpu6050_publisher.py
  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

Ensure You Have the Following Lines for Dependencies:

find_package(catkin REQUIRED COMPONENTS
  rospy
  std_msgs
  sensor_msgs
  tf
)

4.4. Build the ROS Package

1. Navigate to Your Catkin Workspace:

cd ~/catkin_ws

2. Build the Workspace:

catkin_make

3. Source the Workspace:

source devel/setup.bash

4. Barely, success.