python - 订阅 IMU 传感器并监测方位值以确定汽车的行驶方向

Question

订阅汽车的 IMU 传感器并监控方向值以确定汽车是直行、左转还是右转，并创建一个每秒在屏幕上输出的程序。
由于方向值是四元数格式，我们需要使用'Euler_from_quaternion'函数将其替换为滚动、俯仰和偏航的欧拉格式。
(仅供引用，用 Python 在 ROS 中运行这个程序)
这是我到目前为止所得到的..

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

根据您的脚本，以下代码是一个建议:

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