Subscribe to IMU sensor and monitor the orientation value to determine the driving direction of the car

Question

Subscribe to car's IMU sensor and monitor the orientation value to determine if the car is going straight, left, or right, and create a program that outputs it on the screen every second. Since the orientation value is in quaternion format, we need to use the 'Euler_from_quaternion' function to replace it with the Euler format of roll, pitch, and yaw. (FYI Running this program in ROS with Python) This is what I have got so far..

#!/usr/bin/env python
import rospy
import time

from sensor_msgs.msg import Imu
from tf.transformations import euler_from_quaternion

Imu_msg = None

def imu_callback(data):
    global Imu_msg
    Imu_msg = [data.orientation.x, data.orientation.y, data.orientation.z, data.orientation.w]


rospy.init_node("Imu_Print")
rospy.Subscriber("imu", Imu, imu_callback)

from time import time
while not rospy.is_shutdown():
    current_time = time.time()- t
    current_yaw = yaw
    if current_time >= 2.0:
        if current_yaw - last_yaw >0:
            print("Left")
        else:
            print("Right")
        t = time.time()
    last_yaw = current_yaw
    
    if Imu_msg == None:
        continue

    (roll, pitch, yaw) = euler_from_quaternion(Imu_msg)
    print('Roll:%.4f, Pitch:%.4f, Yaw:%.4f' % (roll, pitch, yaw))
    time.sleep(1.0)

Answer 1

Based on your script, this following code is a suggestion:

#!/usr/bin/env python3
import rospy

from time import time
from sensor_msgs.msg import Imu
from tf.transformations import euler_from_quaternion

class DetectorDrivingDirection: 

        DIRECTION_UNKNOWN = -10
        DIRECTION_LEFT = -1
        DIRECTION_STRAIGHT = 0
        DIRECTION_RIGHT = 1

        DIRECTIONS_DEF = {
            DIRECTION_UNKNOWN: "unknown",
            DIRECTION_LEFT: "left",
            DIRECTION_STRAIGHT: "straight",
            DIRECTION_RIGHT: "right"
        }

        def __init__(self,timeout_in_s=2.0): 
            self._const_timeout_in_s = timeout_in_s
            self._imu_msg = Imu()

            self._last_yaw = 0
            self._last_time = time()


        def _get_current_yaw(self):
            quaternion_list = [self._imu_msg.orientation.x, 
                self._imu_msg.orientation.y, 
                self._imu_msg.orientation.z, 
                self._imu_msg.orientation.w]
            euler_list = euler_from_quaternion(quaternion_list)
            return euler_list[2]

        def callback_imu(self,msg): 
            self._imu_msg = msg

        def reset(self):
            yaw_current = self._get_current_yaw()
            self._last_yaw = yaw_current
            self._last_time = time()

        def is_timeout(self):
            return (time() - self._last_time) > self._const_timeout_in_s

        def get_direction(self):
            yaw_current = self._get_current_yaw()
            diff = yaw_current - self._last_yaw
            direction = DetectorDrivingDirection.DIRECTION_UNKNOWN
            if(diff > 0):
                direction = DetectorDrivingDirection.DIRECTION_RIGHT
            elif(diff < 0):
                direction = DetectorDrivingDirection.DIRECTION_LEFT
            else: 
                direction = DetectorDrivingDirection.DIRECTION_STRAIGHT
            return direction


if __name__=="__main__":
    rospy.init_node("node_direction_detector")
    detector_driving_direction = DetectorDrivingDirection()
    rospy.Subscriber("imu", Imu, detector_driving_direction.callback_imu)

    rate = rospy.Rate(10)
    while(not rospy.is_shutdown()):
        if(detector_driving_direction.is_timeout()):
            direction = detector_driving_direction.get_direction()
            direction_def = detector_driving_direction.DIRECTIONS_DEF[direction]
            rospy.loginfo(direction_def)
            detector_driving_direction.reset()
        rate.sleep()