ROS2中IMU话题的发布及可视化

环境：Ubuntu 20.04，ROS2 Foxy
传感器：维特智能BWT901CL

代码是从维特智能的示例代码修改的，实现基本的加速度、角速度和角度读取，发布IMU消息。这个传感器还支持磁场输出等功能，后面再加上吧。
IMU消息的说明可以看这里sensor_msgs/Imu Message

This is a message to hold data from an IMU (Inertial Measurement Unit)
Accelerations should be in m/s^2 (not in g’s), and rotational velocity should be in rad/sec
Header header
geometry_msgs/Quaternion orientation
float64[9] orientation_covariance # Row major about x, y, z axes
geometry_msgs/Vector3 angular_velocity
float64[9] angular_velocity_covariance # Row major about x, y, z axes
geometry_msgs/Vector3 linear_acceleration
float64[9] linear_acceleration_covariance # Row major x, y z

用sudo chmod 777 /dev/ttyUSB0 修改串口权限。
RViz2进行可视化，需用sudo apt-get install ros-foxy-imu-tools安装用于IMU消息显示的插件，之后在 Add -> By topic 中选择IMU消息即可。

import rclpy    # ROS2 Python Client Library
from rclpy.node import Node    # ROS2 Node
from sensor_msgs.msg import Imu
import serial # Usart Library
from tf_transformations import quaternion_from_eulerclass ImuNode(Node):def __init__(self,name):super().__init__(name)self.get_logger().info("imu node init") self.a = [0.0] * 3 # m/s^2self.w = [0.0] * 3 # rad/sself.angle = [0.0] * 3 # radself.ser = serial.Serial('/dev/ttyUSB0', 115200, timeout=0.5)if self.ser.is_open:print("open success")else:print("open failed")self.publisher = self.create_publisher(Imu, 'imu_data', 1)self.timer = self.create_timer(0.001, self.timer_callback)def timer_callback(self):datahex = self.ser.read(33)self.DueData(datahex)imu_data = Imu()imu_data.header.frame_id = 'map'imu_data.header.stamp = self.get_clock().now().to_msg()imu_data.linear_acceleration.x = self.a[0]imu_data.linear_acceleration.y = self.a[1]imu_data.linear_acceleration.z = self.a[2]imu_data.angular_velocity.x = self.w[0]imu_data.angular_velocity.y = self.w[1]imu_data.angular_velocity.z = self.w[2]q = quaternion_from_euler(self.angle[0], self.angle[1], self.angle[2])imu_data.orientation.x = q[0]imu_data.orientation.y = q[1]imu_data.orientation.z = q[2]imu_data.orientation.w = q[3]self.publisher.publish(imu_data) def DueData(self,inputdata):   #新增的核心程序，对读取的数据进行划分，各自读到对应的数组里FrameState = 0  #通过0x后面的值判断属于哪一种情况Bytenum = 0     #读取到这一段的第几位CheckSum = 0    #求和校验位ACCData = [0.0] * 8GYROData = [0.0] * 8AngleData = [0.0] * 8for data in inputdata:  #在输入的数据进行遍历if FrameState == 0:   #当未确定状态的时候，进入以下判断if data == 0x55 and Bytenum == 0: #0x55位于第一位时候，开始读取数据，增大bytenumCheckSum = dataBytenum = 1continueelif data == 0x51 and Bytenum == 1:#在byte不为0 且 识别到 0x51 的时候，改变frameCheckSum += dataFrameState = 1Bytenum = 2elif data == 0x52 and Bytenum == 1: #同理CheckSum += dataFrameState = 2Bytenum=2elif data == 0x53 and Bytenum == 1:CheckSum += dataFrameState = 3Bytenum = 2elif FrameState == 1: # acc_x   #已确定数据代表加速度   if Bytenum < 10:            # 读取8个数据ACCData[Bytenum-2] = data # 从0开始CheckSum += dataBytenum += 1else:if data == (CheckSum & 0xff):  #假如校验位正确self.a = self.get_acc(ACCData)CheckSum = 0 #各数据归零，进行新的循环判断Bytenum = 0FrameState = 0elif FrameState == 2: # gyro                if Bytenum < 10:GYROData[Bytenum-2] = dataCheckSum += dataBytenum += 1else:if data == (CheckSum & 0xff):self.w = self.get_gyro(GYROData)CheckSum = 0Bytenum = 0FrameState = 0elif FrameState == 3: # angle               if Bytenum < 10:AngleData[Bytenum-2] = dataCheckSum += dataBytenum += 1else:if data == (CheckSum & 0xff):self.angle = self.get_angle(AngleData)#d = self.a + self.w + self.angle#print("a(m/s^2):%10.3f %10.3f %10.3f w(rad/s):%10.3f %10.3f %10.3f Angle(rad):%10.3f %10.3f %10.3f"%d)CheckSum = 0Bytenum = 0FrameState = 0def get_acc(self,datahex):  axl = datahex[0]                                        axh = datahex[1]ayl = datahex[2]                                        ayh = datahex[3]azl = datahex[4]                                        azh = datahex[5]       k_acc = 16.0    acc_x = (axh << 8 | axl) / 32768.0 * k_accacc_y = (ayh << 8 | ayl) / 32768.0 * k_accacc_z = (azh << 8 | azl) / 32768.0 * k_accif acc_x >= k_acc:acc_x -= 2 * k_accif acc_y >= k_acc:acc_y -= 2 * k_accif acc_z >= k_acc:acc_z-= 2 * k_acc g = 9.8acc_x *= gacc_y *= gacc_z *= g    return acc_x,acc_y,acc_zdef get_gyro(self,datahex):                                      wxl = datahex[0]                                        wxh = datahex[1]wyl = datahex[2]                                        wyh = datahex[3]wzl = datahex[4]                                        wzh = datahex[5]k_gyro = 2000.0    gyro_x = (wxh << 8 | wxl) / 32768.0 * k_gyrogyro_y = (wyh << 8 | wyl) / 32768.0 * k_gyrogyro_z = (wzh << 8 | wzl) / 32768.0 * k_gyroif gyro_x >= k_gyro:gyro_x -= 2 * k_gyroif gyro_y >= k_gyro:gyro_y -= 2 * k_gyroif gyro_z >=k_gyro:gyro_z-= 2 * k_gyrogyro_x = gyro_x * 3.1415926 / 180.0gyro_y = gyro_z * 3.1415926 / 180.0gyro_z = gyro_y * 3.1415926 / 180.0return gyro_x,gyro_y,gyro_zdef get_angle(self,datahex):                                 rxl = datahex[0]                                        rxh = datahex[1]ryl = datahex[2]                                        ryh = datahex[3]rzl = datahex[4]                                        rzh = datahex[5]k_angle = 180.0     angle_x = (rxh << 8 | rxl) / 32768.0 * k_angleangle_y = (ryh << 8 | ryl) / 32768.0 * k_angleangle_z = (rzh << 8 | rzl) / 32768.0 * k_angleif angle_x >= k_angle:angle_x -= 2 * k_angleif angle_y >= k_angle:angle_y -= 2 * k_angleif angle_z >=k_angle:angle_z -= 2 * k_angleangle_x = angle_x * 3.1415926 / 180.0angle_y = angle_y * 3.1415926 / 180.0angle_z = angle_z * 3.1415926 / 180.0return angle_x,angle_y,angle_zdef main(args=None):rclpy.init(args=args)wit_node = ImuNode("wit_node")rclpy.spin(wit_node)rclpy.shutdown()

ROS2中IMU话题的发布及可视化相关推荐

ROS2手写接收IMU数据(Imu)代码并发布
目录前言接收IMU数据 IMU的串口连接问题 python接收串口数据 python解析数据 ROS2发布IMU数据可视化IMU数据效果前言在前面测试完了单独用激光雷达建图之后,一直想把 ...
维特智能IMU 接入ROS发布IMU数据类型话题
#维特智能IMU 接入ROS发布IMU数据类型话题 1.准备工作 1.1安装串口功能包 sudo apt-get install ros-melodic-serial 1.2创建功能包 $ cd ~/ ...
当定频神器爱上多线程|ROS2定频话题发布Demo
我们想让话题按照某个固定的速率进行发布,可以采用ROS2中的定时神器Rate,不清楚Rate的小伙伴可以看看小鱼的这篇文章:ROS中的定频神器你会用吗为了能够一直循环使用rate,我们单独开一个线程 ...
ROS2 基础概念话题
ROS2 基础概念话题 1. Topics 2. rqt_graph 3. 话题 4. 话题类型 5. 话题发布 6. 话题频率 1. Topics 话题是节点交换消息的总线节点可以向任意数量的话 ...
ROS2教程 04 话题Topic
一.ROS2 话题Topic 示意图 ROS2的Topic可以是一对一,一对多,多对一,多对多,同一个话题Topic可以被不同的节点Node订阅与发布二.与ros1区别 topic下有多种命令,以下 ...
ROS系列——mavros功能包中常用话题和服务介绍，包括消息名称、类型、头文件、成员变量、示例代码
ROS系列--mavros功能包中常用话题和服务介绍,包括消息名称.类型.头文件.成员变量.示例代码官方链接常用话题订阅 1.1 系统状态 1.2 GPS数据 1.3 本地位置 1.4 三轴速度 ...
恒生与中国信通院联合发布《证券行业分布式核心系统SRE运维白皮书》
在互联网金融模式的变革和冲击下,金融机构面临着海量客户管理.业务场景快速增长.金融服务和产品多样化等挑战. 为应对不断增加的技术创新需求,证券行业核心系统正逐步从传统IT集约型架构向支持敏捷开发.弹性 ...
本周AI热点回顾：Github私有库无限协作、飞腾适配百度昆仑AI处理器、OpenAI发布神经网络可视化库
01 Github私有库无限协作,大 AI 模型随便放 4月14日,GitHub 的 CEO Nat Friedman 在官网上发布声明说, "我们很高兴宣布,我们将为所有 GitHub 用 ...
zed2i相机中imu内参的标定及外参标定
zed2i中imu内参的标定参考: https://blog.csdn.net/weixin_42681311/article/details/126109617 https://blog.csdn ...

ROS2中IMU话题的发布及可视化

ROS2中IMU话题的发布及可视化相关推荐

最新文章

热门文章