无人机SLAM工作日志

imu传感器IM648在ubuntu22.04上的蓝牙配对

之前购买的imu传感器BWT901BLE无法和电脑进行蓝牙连接，并且无法在Ubuntu里跑ros例程。因此，购买了新的imu传感器IM648。IM648可以将数据发送给node节点并且能将节点数据作为消息发送给ROS主题。IM648的蓝牙通信是基于GATT协议完成的。因此，需要在Ubuntu上实现GATT协议通信。

在Windows上打开IM648的GUI操作界面查看其MAC号

在windows系统中打开bluez_imu文件夹，打开可执行文件IMU_2.33_Release.exe。可以看到如下的GUI界面。

此时，打开IM648的蓝牙按钮，看到蓝灯间隔一段时间闪烁一次。

点击配对

这个时候，可以看到右侧的出现了MAC号，我们的设备是83:9A:92:AF:20:DE。

对于MAC地址，这是一个硬件固有的地址，不会随着端口或者链接设备的改变而改变。在GATT协议里应该是作为发送数据包的一部分而存在的。

在Ubuntu中实现GATT协议通信

在bluez_imu文件夹中创建一个叫做turn_on.sh的文件

打开终端，输入如下命令：

1
2
3

cd ..
cd bluez_imu
nano turn_on.sh

打开turn_pn.sh，在里面输入如下代码：

1	python3 ~/bluez_imu/imu_bluetooth_Python_ForLinux_20230731/imu_bluetooth_Python/imu_node.py 83:9A:92:AF:20:DE

这里最好补全路径。

之前报错情况：由于路径没有写对，把前面的home写成了media，提示报错信息：MAC地址和adpater_name不正确。

在此之后，我们检查imu_node.py的代码。在里面，会出现如下代码：

host = args.host_ip
port = 6666
sock = None
print("host ip: ",host)
if host is not None:
    try:
        sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        sock.connect((host, port))
    except:
        print("Could not make a connection to the server")
        input("Press enter to quit")
        sys.exit(0)


print("Connecting bluetooth ...")

manager = gatt.DeviceManager(adapter_name='hci0')
device = AnyDevice(manager=manager, mac_address=args.mac_address)
device.sock_pc = sock
if host is None:
    device.parse_imu_flage = True

device.connect()

manager.run()

其中，出现了adapter_name，这里不需要修改，默认的蓝牙适配器名称都是hci0。之前吃过亏，以为这里应该改成IM648。然而这里并不需要。

在完成检查之后，回到bluez_imu文件夹，打开命令行，运行如下命令：

1	./turn_on.sh

此时等待蓝牙链接。等待蓝牙链接后，IM648模块就会出现连续两次频闪，并且在终端中可以看见imu信息在不断的发送和更新。

imu传感器IM648将node节点数据发送给ROS2消息

对于已有的imu_node.py文件，其内部代码包括以下两个部分：

Class类的建立：class AnyDevice(gatt.Device):

该类用于imu数据接收、imu参数设置、imu数据处理、imu数据储存。
GATT协议通信：

该部分用于imu模块和上位机进行蓝牙通信。

对于第一个Class类的建立，我们将修改其中的代码，将imu数据储存进ROS2中给定的变量之中。为此，我借鉴了ROS.txt和tello的ROS包中node.py中的代码。

创建node节点作为ROS2的接收节点

在class AnyDevice(gatt.Device):下，添加代码：

1	node = rclpy.create_node("im648")

需要注意的是，我们需要包含rclpy库：

1 2	import rclpy from rclpy.node import Node

同时，要对其进行初始化：

1	rclpy.init(args=None)

修改数据处理代码将数据存储至ROS2中给定的变量

首先，我们需要在if buf[0] == 0x11:下设置一个msg变量名。这个变量名用于调用ROS2中的Imu()成员：

1	msg = Imu()

同时，设置时间戳、帧id：

1 2	msg.header.stamp=self.node.get_clock().now().to_msg() msg.header.frame_id="imu648_node"

随后，我们需要对线性加速度、角速度、四元数，三个数据处理后的数据存储到ROS2给定的变量中。代码如下：

L =7 # 从第7字节开始根据 订阅标识tag来解析剩下的数据
# 去除重力下的线加速度
if ((ctl & 0x0001) != 0):
    tmpX = np.short((np.short(buf[L+1])<<8) | buf[L]) * scaleAccel; L += 2 
    print("\taX: %.3f"%tmpX); # x加速度aX
    tmpY = np.short((np.short(buf[L+1])<<8) | buf[L]) * scaleAccel; L += 2 
    print("\taY: %.3f"%tmpY); # y加速度aY
    tmpZ = np.short((np.short(buf[L+1])<<8) | buf[L]) * scaleAccel; L += 2
    print("\taZ: %.3f"%tmpZ); # z加速度aZ

    imu_dat[0] = float(tmpX)
    imu_dat[1] = float(tmpY)
    imu_dat[2] = float(tmpZ)
    msg.linear_acceleration.x = imu_dat[0]
    msg.linear_acceleration.y = imu_dat[1]
    msg.linear_acceleration.z = imu_dat[2]

# 包含重力下的线加速度
print(" ")
if ((ctl & 0x0002) != 0):
    tmpX = np.short((np.short(buf[L+1])<<8) | buf[L]) * scaleAccel; L += 2
    print("\tAX: %.3f"%tmpX) # x加速度AX
    tmpY = np.short((np.short(buf[L+1])<<8) | buf[L]) * scaleAccel; L += 2
    print("\tAY: %.3f"%tmpY) # y加速度AY
    tmpZ = np.short((np.short(buf[L+1])<<8) | buf[L]) * scaleAccel; L += 2
    print("\tAZ: %.3f"%tmpZ) # z加速度AZ

    imu_dat[3] = float(tmpX)
    imu_dat[4] = float(tmpY)
    imu_dat[5] = float(tmpZ)
    msg.linear_acceleration.x = imu_dat[3]
    msg.linear_acceleration.y = imu_dat[4]
    msg.linear_acceleration.z = imu_dat[5]

# 角速度
print(" ")
if ((ctl & 0x0004) != 0):
    tmpX = np.short((np.short(buf[L+1])<<8) | buf[L]) * scaleAngleSpeed; L += 2 
    print("\tGX: %.3f"%tmpX) # x角速度GX
    tmpY = np.short((np.short(buf[L+1])<<8) | buf[L]) * scaleAngleSpeed; L += 2 
    print("\tGY: %.3f"%tmpY) # y角速度GY
    tmpZ = np.short((np.short(buf[L+1])<<8) | buf[L]) * scaleAngleSpeed; L += 2
    print("\tGZ: %.3f"%tmpZ) # z角速度GZ

    imu_dat[6] = float(tmpX)
    imu_dat[7] = float(tmpY)
    imu_dat[8] = float(tmpZ)
    msg.angular_velocity.x = imu_dat[6] * 0.0174532925
    msg.angular_velocity.y = imu_dat[7] * 0.0174532925
    msg.angular_velocity.z = imu_dat[8] * 0.0174532925

# 四元数
print(" ")
if ((ctl & 0x0020) != 0):
    tmpAbs = np.short((np.short(buf[L+1])<<8) | buf[L]) * scaleQuat; L += 2
    print("\tw: %.3f"%tmpAbs); # w
    tmpX =   np.short((np.short(buf[L+1])<<8) | buf[L]) * scaleQuat; L += 2
    print("\tx: %.3f"%tmpX); # x
    tmpY =   np.short((np.short(buf[L+1])<<8) | buf[L]) * scaleQuat; L += 2
    print("\ty: %.3f"%tmpY); # y
    tmpZ =   np.short((np.short(buf[L+1])<<8) | buf[L]) * scaleQuat; L += 2
    print("\tz: %.3f"%tmpZ); # z

    imu_dat[15] = float(tmpAbs)
    imu_dat[16] = float(tmpX)
    imu_dat[17] = float(tmpY)
    imu_dat[18] = float(tmpZ)

    msg.orientation.w = imu_dat[15]
    msg.orientation.x = imu_dat[16]
    msg.orientation.y = imu_dat[17]
    msg.orientation.z = imu_dat[18]