Dora-rs 机器人框架学习教程（1）——IMU传感器驱动

目录

 前言

1. 100D4驱动

1.1 驱动代码

1.2 调试代码

1.3 编写dataflow.yml文件

2 使用

2.1 使用方式

参考资料

---

 前言

目标：编写一个python节点读取IMU传感器。
程序的基本思路是新建一个python dora节点，在该节点中调用串口读取传感器数据，并将数据发布到dora中去。
本文使用的IMU传感器是100D4-030076。

dora的安装参考：Dora-rs 机器人框架学习教程（1）—— Dora-rs安装-CSDN博客

---

1. 100D4驱动

注意：python版本和dora版本

python-3.11

dora-cli 0.3.0

1.1 驱动代码

新建在op_1文件下创建op_1.py文件，将以下内容复制到文件中

import pickle
from typing import Callable
import smbus
from dora import DoraStatus
import serial
from typing import Callable
from dora import DoraStatus
# from DoraNmeaDriver_utils import DoraNMEADriver, Timestamp
from transforms3d._gohlketransforms import quaternion_from_euler
import math
import pickle

serial_port = "/dev/ttyUSB0"
serial_baud = 115200

class Operator:
    """
    Template docstring
    """

    def __init__(self):
        """Called on initialisation"""
        self.data_length = 40
        self.imu_yaw_calibration = 0.0
        self.roll=0
        self.pitch=0
        self.yaw=0
        self.seq=0
        self.accel_factor = 9.806 / 256.0    # sensor reports accel as 256.0 = 1G (9.8m/s^2). Convert to m/s^2.
        self.degrees2rad = math.pi/180.0
        self.accel_data = {'x': 0, 'y': 0, 'z': 0}
        self.gyro_data = {'x': 0, 'y': 0, 'z': 0}
        """
        打开串口读数据，校验、解析后，send_out解析得到的消息类型
        """
        # 打开串口读取数据
        try:
            self.IMU = serial.Serial(port=serial_port, baudrate=serial_baud, timeout=1)
            self.tmp = self.IMU.write(('\xA5\x5A\x04\x01\x05\xAA' + chr(13)).encode())
        except serial.SerialException as ex:
            print(
                "Could not open serial port: I/O error({0}): {1}".format(
                    ex.errno, ex.strerror
                )
            )
        pass
 
    
    def H_(self,data):
        re = ord(chr(data)) * 256
        if re > 32767:
            return re - 65536
        else:
            return re
        
    def L_(self,data):
        re = ord(chr(data))
        return re
    
    def on_event(
            self,
            dora_event: dict,
            send_output: Callable[[str, bytes], None],
    ) -> DoraStatus:
        if dora_event["type"] == "INPUT":    
            self.data = self.IMU.read_all()
            if len(self.data) > 0:
                if 0xa5 == self.data[0] and 0x5a == self.data[1] and 0xaa == self.data[len(self.data)-1]:
                    yaw_deg = ( self.H_(self.data[3]) + self.L_(self.data[4]) )/10.0
                    yaw_deg = self.imu_yaw_calibration
                    if yaw_deg > 180.0:
                        yaw_deg = yaw_deg - 360.0
                    if yaw_deg < -180.0:
                        yaw_deg = yaw_deg + 360.0

                    self.yaw    = self.degrees2rad * yaw_deg
                    self.pitch  = self.degrees2rad * ( self.H_(self.data[7]) + self.L_(self.data[8]) )/10.0
                    self.roll   = self.degrees2rad * ( self.H_(self.data[5]) + self.L_(self.data[6]) )/10.0
                    self.accel_data['x'] = 9.806 * ( self.H_(self.data[9]) + self.L_(self.data[10]) ) / 16384.0
                    self.accel_data['y'] = 9.806 * ( self.H_(self.data[11]) + self.L_(self.data[12]) ) / 16384.0
                    self.accel_data['z'] = 9.806 * ( self.H_(self.data[13]) + self.L_(self.data[14]) ) / 16384.0

                    self.gyro_data['x'] = self.degrees2rad * ( self.H_(self.data[15]) + self.L_(self.data[16]) ) / 32.8
                    self.gyro_data['y'] = self.degrees2rad * ( self.H_(self.data[17]) + self.L_(self.data[18]) ) / 32.8
                    self.gyro_data['z'] = self.degrees2rad * ( self.H_(self.data[19]) + self.L_(self.data[20]) ) / 32.8
                    imu_dict = {
                        "accel_data": {
                            "x": self.accel_data['x'],
                            "y": self.accel_data['y'],
                            "z": self.accel_data['z'],
                        },
                        "gyro_data": {
                            "x": self.gyro_data['x'],
                            "y": self.gyro_data['y'],
                            "z": self.gyro_data['z'],
                        }
                    }
                    print(imu_dict)
                    serialized_data = pickle.dumps(imu_dict)
                    send_output("Imu100D4",
                                serialized_data,
                                dora_event["metadata"])
        
        return DoraStatus.CONTINUE

    def __del__(self):
        """Called before being deleted"""
        pass

1.2 调试代码

新建在node_1文件下创建node_1.py文件，将以下内容复制到文件中

#!/usr/bin/env python
# -*- coding: utf-8 -*-

# from dora import Node

# node = Node()

# event = node.next()
# if event["type"] == "INPUT":
#     print(
#         f"""Node received:
#         id: {event["id"]},
#         value: {event["data"]},
#         metadata: {event["metadata"]}"""
#     )


from typing import Callable
from dora import DoraStatus
import pickle


class Operator:
    """
    反序列化后，输出各类消息内容
    """

    def __init__(self):
        pass
        
    def on_event(
        self,
        dora_event: dict,
        send_output: Callable[[str, bytes], None],
    ) -> DoraStatus:
        if dora_event["type"] == "INPUT":
            return self.on_input(dora_event, send_output)
        return DoraStatus.CONTINUE

    def on_input(
        self,
        dora_input: dict,
        send_output: Callable[[str, bytes], None],
    ):
        # print(dora_input["id"])
        if "Imu100D4" == dora_input["id"]:
            dora_input = dora_input["value"]
            # print(dora_input["value"])
            dora_input_bytes = bytes(dora_input.to_pylist())
            self.receDoraSentence = pickle.loads(dora_input_bytes)
            print(self.receDoraSentence)


        return DoraStatus.CONTINUE

1.3 编写dataflow.yml文件

该文件是配置dora的数据流，op_1.py读取传感器数据,node_1.py进行数据打印显示。

nodes:
  - id: op_1
    operator:
      python: op_1/op_1.py
      inputs:
        tick: dora/timer/millis/10
      outputs:
        - Imu100D4

  - id: custom-node_1
    operator:
      python: node_1/node_1.py
      inputs:
#        tick: dora/timer/secs/1
        Imu100D4: op_1/Imu100D4

2 使用

2.1 使用方式

1.给串口权限

注意：根据你使用的串口不一样，给的权限也不一样，例如/dev/ttyUSB0，且代码op_1.py中代码串口要和此保持一致。

sudo chmod 777 /dev/ttyUSB0

2.启动dora

dora up

3.执行dataflow.yml文件并打印数据

注意：是在你自己dora工作环境下执行，例如

cd /home/crp/dora_project/dora-rs/dora-hardware/vendors/imu/100D4
dora start dataflow.yml --name first-dataflow
# Output: c95d118b-cded-4531-a0e4-cd85b7c3916c
dora logs first-dataflow custom_node_1

参考资料

[1] https://github.com/dora-rs/dora
[2] https://github.com/dora-rs/dora-hardware/tree/main/vendors/imu/mpu6050