Robotics

Raspberrypi4 and IMU MPU9250 Interfacing ด้วย ROS

31 มกราคม 2564

Raspberrypi4 with MPU9250 Interfacing with ROS (การเชื่อมต่อ IMU MPU9250 กับ Raspberry Pi ด้วย ROS )

ในการทำหุ่นยนต์ ROS ที่สามารถทำ SLAM (Simultaneous Localization And Mapping) ได้นั้น สิ่งสำคัญเลยคือการคำนวณ Odometry การเคลื่อนที่ของหุ่นยนต์ ว่าหุ่นยนต์มีการเปลี่ยนแปลงการเคลื่อนที่ไปอย่างไร แล้วนำไปใช้ในการสร้างแผนที่ การได้มาซึ่ง Odometry นั้นสามารถหาได้จาก sensor อย่าง encoder ของล้อของหุ่นยนต์ เป็นต้น แต่ในหุ่นยนต์ที่ต้องการข้อมูลของ odometry ที่มีความคลาดเคลื่อนน้อยลง ก็จำเป็นต้องหาข้อมูลจาก sensor อื่นมาชวยกันชดเชย error ให้น้อยลงไป โดยนำมา fusion กัน ตัว sensor ที่นิยมนำมาเพิ่มเช่น GPS, Optical flow (สำหรับ drone), LIDAR และ IMU (วัด angular velocity, acceleration, magneto) เป็นต้น

แต่ก่อนที่จะนำข้อมูลไป fusion ได้นั้น ก็ต้องหาทางดึงข้อมูลออกมาก่อน โดยหัวข้อนี้จะขอนำเสนอการดึงข้อมูลจาก IMU ที่ต่อกับ Raspberry Pi4 ออกมาผ่าน ROS กันครับ โดยมีขั้นตอนดังต่อไปนี้

STEP1 : ต่อวงจรดังในรูปต่อไปนี้

ในการต่อ IMU MPU9250 เข้ากับ Raspi4 นั้น จะต่อขาของ IMU เข้ากับขาของ Raspi 4 ดังนี้

IMU MPU9250 ---> Raspberry Pi4

VCC ---> 5V

GND ---> GND

SCL , ECL ---> SCL (GPIO3)

SDA, EDA ---> SDA (GPIO2)

STEP2 : Raspberry Pi4 Setup

ในการส่งของมูลระหว่าง IMU กับ Raspberry Pi นั้น จะใช้พอร์ค I2C (I square C) ในการส่งข้อมูล ดังนั้นจะต้องเปิด port I2C ของ raspberry pi ให้เรียบร้อยก่อนดังนี้

sudo raspi-config

STEP3 : ติดตั้ง Driver ของ MPU 9250

หลังจากต่อวงจรเสร็จเรียบร้อยและทำการตั้งค่า raspberry pi เสร็จเรียบร้อยแล้ว ให้ทำการติดตั้ง driver ของ MPU9250 เข้าไปใน Raspi4 ของเราโดยใช้ command

pip install FaBo9Axis_MPU9250

รอจนการติดตั้งเสร็จเรียบร้อย จากนั้น ทำการตรวจสอบว่ามีการเชื่อมต่อ IMU เข้ากับ Raspi4 แล้วหรือยังโดยใช้ command :

sudo i2cdetect -y 1

จะมี address ของ MPU9250 แสดงดังในรูป โดยใน IMU ค่า address ของ MPU9250 เป็น 0x68 หากแสดงไม่ครบอาจจะเกิดจากความผิดพลาดต่างๆ ให้ลองตรวจสอบการเชื่อมต่อของสาย และการเปิด port ของ raspberry pi

IMU i2c detect window
STEP4: สร้าง node สำหรับ publish ค่าของ IMU
หลังจากที่ติดตั้งไดรเวอร์และเชื่อมต่อ IMU เรียบร้อยแล้ว เราจำเป็นที่จะต้องเขียนโปรแกรม สร้าง node สำหรับรับค่า IMU มา แล้ว Publish ค่าที่ได้มาจาก IMU ออกมาให้ node อื่นสามารถนำไปใช้งานได้ โดยภาษาที่ใช้เขียนโปรแกรมในครั้งนี้จะเป็นภาษา Python โดยใน ROS หรือระบบปฏิบัติการหุ่นยนต์นั้นจะมีรูปแบบการสร้าง Node อยู่ โดยสามารถศึกษาการสร้าง ROS package และ ROS Node ได้จาก ที่นี่ และ ลิงค์นี้ ตามลำดับ ก่อน จากนั้นให้ทำการสร้าง workspace ขึ้นมาก่อน
mkdir catkin_ws cd catkin_ws catkin_make source devel/setup.bash

จากนั้นทำการสร้าง package ขึ้นมา 1 package
cd ~/catkin_ws/src catkin_create_pkg imu_test std_msgs rospy sensor_msgs geometry_msgs

จากนั้นทำการ build ตัว package ด้วย command
cd ~/catkin_ws catkin_make

เข้าไปที่ folder src ใน package imu_test แล้วสร้างไฟล์ imu.py
cd ~/catkin_ws/src/imu_test/src touch imu.py

แล้วนำโค้ดต่อไปนี้ใส่ลงไปในไฟล์ imu.py
#!/usr/bin/env python import rospy from sensor_msgs.msg import Imu from geometry_msgs.msg import Quaternion, Vector3, PoseStamped, WrenchStamped import FaBo9Axis_MPU9250 import time import sys mpu9250 = FaBo9Axis_MPU9250.MPU9250() def imu_talker(): pub = rospy.Publisher('imu', Imu, queue_size = 5) rospy.init_node('aexros_imu', anonymous = False) rate = rospy.Rate(10) while not rospy.is_shutdown(): gyro = mpu9250.readGyro() accel = mpu9250.readAccel() groll = gyro['x'] gpitch = gyro['y'] gyaw = gyro['z'] ax = accel['x'] ay = accel['y'] az = accel['z'] imu_msg = Imu() #mu_msg.header.stamp = rospy.rostime.Time() imu_msg.angular_velocity.x = groll imu_msg.angular_velocity.y = gpitch imu_msg.angular_velocity.z = gyaw imu_msg.linear_acceleration.x = ax imu_msg.linear_acceleration.y = ay imu_msg.linear_acceleration.z = az imu_str = "gx = {} gy = {} gz = {} ax= {} ay = {} az = {}" rospy.loginfo(imu_str.format(groll,gpitch,gyaw,ax,ay,az)) pub.publish(imu_msg) rate.sleep() if __name__ == '__main__': try: imu_talker() except rospy.ROSInterruptException: pass

จากนั้นให้ทำการใส่ permission ให้ไฟล์ imu.py สามารถ execute ได้ และทำการ build ตัว node ที่สร้างขึ้นโดยใช้ command
chmod +x imu.py cd ~/catkin_ws catkin_make

**STEP5 : ทดสอบการทำงาน**

จากนั้นให้ทำการเปิดใช้งาน roscore
roscore

เปิด terminal อีก 1 terminal แล้วทดสอบการทำงานของ imu publisher node ด้วย rosrun
rosrun imu_test imu.py

เมื่อรัน node ของ imu แล้วผลที่ได้ออกมาจะเป็นดังในรูปภาพดังต่อไปนี้ (ในรูปภาพผมใช้ ชื่อ package เป็น imu_9250 และชื่อไฟล์เป็น aexros_imu.py)

และหากตรวจสอบ topic ที่มีการใช้งานอยู่ด้วย command
rostopic list

จะมี topic ของ imu แสดงอยู่ด้วย ซึ่งเราสามารถสร้าง node มา subscribe ค่าของ imu จาก topic ชื่อ /imu ไปใช้ได้เลยครับผม

เมื่อได้ค่า IMU แล้วก็สามารถนำไป fusion กับข้อมูลจาก sensor ตัวอื่นได้ครับ และโชคดีที่ใน ROS มีตัว package ที่ช่วยในการทำ sensor fusion ด้วย คือ Package ที่ชื่อว่า ROS robot localization ซึ่งใช้งานได้ไม่ยาก มี filter ต่างๆ เช่น EKF (Extended Kalman Filters) ไว้ให้แล้วด้วย ลองศึกษาดูกันนะครับ ถ้ามีโอกาสผมอาจจะนำเสนอวิธีการตั้งค่าการใช้งาน package ดังกล่าวครับ แต่วันนี้ขอลาไปก่อนนะครับผม

IMU i2c detect window
STEP4: สร้าง node สำหรับ publish ค่าของ IMU
หลังจากที่ติดตั้งไดรเวอร์และเชื่อมต่อ IMU เรียบร้อยแล้ว เราจำเป็นที่จะต้องเขียนโปรแกรม สร้าง node สำหรับรับค่า IMU มา แล้ว Publish ค่าที่ได้มาจาก IMU ออกมาให้ node อื่นสามารถนำไปใช้งานได้ โดยภาษาที่ใช้เขียนโปรแกรมในครั้งนี้จะเป็นภาษา Python โดยใน ROS หรือระบบปฏิบัติการหุ่นยนต์นั้นจะมีรูปแบบการสร้าง Node อยู่ โดยสามารถศึกษาการสร้าง ROS package และ ROS Node ได้จาก ที่นี่ และ ลิงค์นี้ ตามลำดับ ก่อน จากนั้นให้ทำการสร้าง workspace ขึ้นมาก่อน
mkdir catkin_ws cd catkin_ws catkin_make source devel/setup.bash

จากนั้นทำการสร้าง package ขึ้นมา 1 package
cd ~/catkin_ws/src catkin_create_pkg imu_test std_msgs rospy sensor_msgs geometry_msgs

จากนั้นทำการ build ตัว package ด้วย command
cd ~/catkin_ws catkin_make

เข้าไปที่ folder src ใน package imu_test แล้วสร้างไฟล์ imu.py
cd ~/catkin_ws/src/imu_test/src touch imu.py

แล้วนำโค้ดต่อไปนี้ใส่ลงไปในไฟล์ imu.py
#!/usr/bin/env python import rospy from sensor_msgs.msg import Imu from geometry_msgs.msg import Quaternion, Vector3, PoseStamped, WrenchStamped import FaBo9Axis_MPU9250 import time import sys mpu9250 = FaBo9Axis_MPU9250.MPU9250() def imu_talker(): pub = rospy.Publisher('imu', Imu, queue_size = 5) rospy.init_node('aexros_imu', anonymous = False) rate = rospy.Rate(10) while not rospy.is_shutdown(): gyro = mpu9250.readGyro() accel = mpu9250.readAccel() groll = gyro['x'] gpitch = gyro['y'] gyaw = gyro['z'] ax = accel['x'] ay = accel['y'] az = accel['z'] imu_msg = Imu() #mu_msg.header.stamp = rospy.rostime.Time() imu_msg.angular_velocity.x = groll imu_msg.angular_velocity.y = gpitch imu_msg.angular_velocity.z = gyaw imu_msg.linear_acceleration.x = ax imu_msg.linear_acceleration.y = ay imu_msg.linear_acceleration.z = az imu_str = "gx = {} gy = {} gz = {} ax= {} ay = {} az = {}" rospy.loginfo(imu_str.format(groll,gpitch,gyaw,ax,ay,az)) pub.publish(imu_msg) rate.sleep() if __name__ == '__main__': try: imu_talker() except rospy.ROSInterruptException: pass

จากนั้นให้ทำการใส่ permission ให้ไฟล์ imu.py สามารถ execute ได้ และทำการ build ตัว node ที่สร้างขึ้นโดยใช้ command
chmod +x imu.py cd ~/catkin_ws catkin_make

**STEP5 : ทดสอบการทำงาน**

จากนั้นให้ทำการเปิดใช้งาน roscore
roscore

เปิด terminal อีก 1 terminal แล้วทดสอบการทำงานของ imu publisher node ด้วย rosrun
rosrun imu_test imu.py

เมื่อรัน node ของ imu แล้วผลที่ได้ออกมาจะเป็นดังในรูปภาพดังต่อไปนี้ (ในรูปภาพผมใช้ ชื่อ package เป็น imu_9250 และชื่อไฟล์เป็น aexros_imu.py)

และหากตรวจสอบ topic ที่มีการใช้งานอยู่ด้วย command
rostopic list

จะมี topic ของ imu แสดงอยู่ด้วย ซึ่งเราสามารถสร้าง node มา subscribe ค่าของ imu จาก topic ชื่อ /imu ไปใช้ได้เลยครับผม

เมื่อได้ค่า IMU แล้วก็สามารถนำไป fusion กับข้อมูลจาก sensor ตัวอื่นได้ครับ และโชคดีที่ใน ROS มีตัว package ที่ช่วยในการทำ sensor fusion ด้วย คือ Package ที่ชื่อว่า ROS robot localization ซึ่งใช้งานได้ไม่ยาก มี filter ต่างๆ เช่น EKF (Extended Kalman Filters) ไว้ให้แล้วด้วย ลองศึกษาดูกันนะครับ ถ้ามีโอกาสผมอาจจะนำเสนอวิธีการตั้งค่าการใช้งาน package ดังกล่าวครับ แต่วันนี้ขอลาไปก่อนนะครับผม

Raspberrypi4 and IMU MPU9250 Interfacing ด้วย ROS

บทความที่เกี่ยวข้อง

ROS2 Web GUI

ROS2 Navigation

การติดตั้ง Ubuntu Mate 20.04 บน Raspberry Pi4